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ABSTRACT 


A  tactical  missile  with  mid- course  requires  the  use  of  an  Inertial 
Navigation  System  (INS).  Steady-state  Kalman  Filters  (SKF)  used  as 
estimators  have  been  proposed  for  use  in  a  Strapdown  INS  that  is  con¬ 
sidered  to  be  cheaper  and  easier  to  implement  than  a  gimbaled  INS. 

This  thesis  further  investigates  the  sensitivity  of  the  SKF  to  in¬ 
accuracies  in  the  filter  parameters  such  as  the  dimensional  stability 
derivatives.  The  analysis  is  expanded  to  explore  the  sensitivity  of  a 
system  of  higher  dimension  created  by  the  augmentation  of  an  additional 
state.  The  study  has  been  performed  by  independently  varying  each  of  the 
filter  parameters  over  a  given  range  and  noting  the  effect  on  the  accu¬ 
racy  of  the  filter.  One  of  the  benefits  of  this  analysis  of  the  rms 
estimate  errors  to  variations  in  the  stability  derivatives  is  that  it 
reveals  which  derivatives  need  to  be  accurately  determined  to  ensure 
stable  flight. 
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I.  INTRODUCTION 


A  tactical  missile  normally  requires  midcourse  guidance  to  ensure 
that  its  trajectory  leads  to  a  specific  target.  A  typical  midcourse 
guidance  law  pre-programmed  strategy  maintains  constant  altitude,  heading 
and  speed.  Such  guidance  is  primarily  effected  by  an  Intertial  Navi¬ 
gation  System  (INS).  In  this  work  two  steady-state  Kalman  Filters  (SKF), 
used  as  estimators  of  the  longitudinal  and  lateral  motion,  constitute 
what  may  be  considered  as  part  of  a  Strapdown  INS  onboard  a  missile  that 
can  be  cheaper  and  easier  to  implement  than  a  gimballed  INS.  The  authors 
of  [Ref.  1]  discuss  the  basic  differences  between  Strapdown  and  gimballed 
Inertial  Navigation  Systems. 

Sensors  on  the  missile  that  the  longitudinal  and  lateral  estimators 
could  use  are  described  by  Maybeck  [Ref.  2]  and  include  laser  rate  gyros, 
doppler  ve loci meters ,  magnetic  compasses,  and  barometric  altimeters.  A 
radar  seeker  could  provide  a  distance  or  range  measurement  or  range  rate. 
Distance  or  position  measurement  could  be  computed  from  a  signal  inserted 
into  the  missile's  INS  from  the  Global  Positioning  System  (GPS)  or 
similar  satellite-based  navigation  system. 

This  work  was  motivated  by  Bryson  [Ref.  3],  where  he  discusses  a 
Strapdown  INS  using  SKF  as  estimators  applied  to  the  model  for  the  DC-8 
airplane.  To  avoid  classification  requirements  and  for  convenience,  the 
model  used  here  is  essentially  the  same  as  that  of  [Ref.  3]  rather  than 
that  of  a  missile. 
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This  thesis  is  a  continuation  of  the  work  done  by  Natal lana  [Ref.  4]. 
It  further  investigates  the  sensitivity  of  the  Kalman  Filter  to  inaccu¬ 
racies  in  the  filter  parameters  or  varation  between  the  filter  model  and 
the  plant  model  for  longitudinal  motion  estimation.  The  differences 
could  be  due  to  model  inaccuracies  or  to  normal  variation  caused  by  a 
changing  flight  environment.  The  sensitivity  of  rms  estimate  errors  to 
inaccuracies  or  differences  in  the  stability  derivatives  is  the  result  of 
interest. 

The  initial  work  conducted  was  to  reproduce  the  results  of  [Ref.  3] 
and  [Ref.  4]  with  the  correct  implementation  of  the  dynamics  in  the 
filter  parameters.  Then  the  results  of  [Ref.  4]  for  the  longitudinal 
motion  estimator  with  incorrect  implementation  of  the  dynamics  in  the 
Kalman  Filter  were  reproduced. 

After  a  distance  measurement  and  associated  system  and  measurement 
noise  parameters  were  added  to  the  model  dynamics,  the  sensitivity  anal¬ 
ysis  was  repeated  for  the  longitudinal  motion  estimator.  The  analysis  of 
the  effect  that  this  distance  input  had  on  the  sensitivity  of  the  nns 
errors  to  inaccuracies  or  differences  in  the  stability  derivatives  of  the 
Kalman  Filter  concluded  the  research  for  this  thesis. 


II.  MODELS  AND  ESTMATION 


A.  KALMAN  FILTER 

Only  a  brief  description  of  the  Kalman  Filter  has  been  included  to 
show  the  particular  formulation  used.  A  more  complete  development  of 
general  theory  is  done  by  Gelb  in  [Ref.  5]. 

1 .  Linear  Dynamic  System 

Consider  the  linear  time  invariant  system  (plant  and  measurement 
models)  given  by  equation  (1)  below,  where  x  represents  the  states  of  the 
system;  z  is  the  measurement;  F  is  the  system  matrix;  r  is  the  driving 
noise  coefficient  matrix;  H  is  the  measurement  scaling  matrix;  and  w  and 
v  are  independent,  zero-mean,  white  gaussian  noise  processes  with  covar¬ 
iance  matrices  Q  and  R  respectively. 

x  =  Fx  +  Fw  (1-a) 

z  =  Hx  +  v  (1-b) 


Mathematically,  Q  and  R  are  represented  by  equation  (2)  as: 

E(w(t)wT(t))  =  Q(t)<7(t-T),  E(w(t))  =  0  (2-a) 

E(v(t)vT(i»  =  R(t)o(t-T),  E(v(t))  =  0  (2-b) 

2.  Continuous  Kalman  Filter 

A  continuous  time  Kalman  Filter  is  described  by  equation  (3) 
where  x  is  the  state  estimate  and  K  is  a  matrix  of  constant  filter  gains. 

x  =  Fx  +  K(z-Hx)  (3) 
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The  implementation  of  the  System  Model  and  the  Kalman  Filter  is  shown  in 
Figure  1. 


MATHEMATICAL  MODEL 

- - — — — - - - i 

SYSTEM  MEASUREMENT  {  KALMAN  FILTER 


Figure  1.  System  Model  and  Kalman  Filter 

The  estimate  error  is  defined  by  equation  (4)  as 

x  -  x  -  x  (4) 

and  the  differential  equation  for  x  is  given  by 

x  =  (F-KH)x  -  Tw  +  Kv  (5) 

The  differential  equations  for  the  states  of  a  linear  system  driven  by 
noise  can  be  expressed  as 


(6) 


<v 

X 

F-KH  0 

x  Kv-Tw 

.X. 

nr  f. 

xj  fw 

The  covariance  of  the  estimate-error,  symbolized  as  P,  is  defined  by 
equation  (7).  It  provides  a  statistical  measure  of  the  uncertainty  in  x. 

P  =  E(xxT)  (7) 

The  diagonal  elements  of  the  covariance  matrix  are  the  root  mean  square 
errors  of  the  state  variables.  Also,  the  trace  of  P  is  the  mean  square 
length  of  the  vector  x.  The  off  diagonal  terms  of  P  indicate  the  degree 
of  cross-correlation  between  the  elements  of  x.  The  covariance  matrix  P 
is  obtained  by  solving  the  linear  Lyapunov  equation  given  by 

P  =  (F-KH)  P  +  P(F-KH)T  +  TQrT  +  KRKT  (8) 

The  eigenvalues  of  the  filter  are  given  by  the  roots  of 

ISI  -  F  +  KHI  =  0  (9) 

B.  STATE  AUGMENTATION  AND  SHAPING  FILTERS 

When  the  system  random  disturbances  are  correlated  in  time,  i.e., 
colored  noise,  it  is  necessary  to  use  their  power  spectral  density  data 
in  order  to  develop  a  mathematical  model  that  produces  an  output  which 
duplicates  the  noise  characteristics  [Ref.  2].  Correlated  random  noises 
are  taken  to  be  state  variables  of  a  ficticious  linear  time  invariant 
system  (usually  called  a  shaping  filter)  which  is  itself  excited  by  white 
gaussian  noise.  Such  a  model  is  given  by  equation  (10)  below,  where  the 
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subscript  f  denotes  filter,  and  n  is  a  nonwhite  (time-correlated)  gaus- 
sian  noise.  The  filter  output  is  used  to  drive  the  system  depicted  by 
Figure  2. 


xf  =  Ffxf  +  Tfw 


2n  “  Hfxf 


( 1 0-a ) 
(10-b) 


The  dimension  of  the  state  vector  (1)  is  increased  by  including  the 
disturbances  as  well  as  a  description  of  the  system  dynamics  behavior  in 
appropriate  rows  of  an  enlarged  F  matrix.  This  enlargement  process  is 
called  state  vector  augmentation. 
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Figure  2.  Shaping  Filter  Generating  Driving  Noise 


The  augmented  state  equation  is  given  by 


01) 
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The  associated  measurement  equation  is 


+  v 


(12) 


C.  SENSITIVITY  TO  PARAMETER  VARIATION 

Observing  the  structure  of  the  Kalman  Filter  illustrated  in  Figure  1, 
the  filter  contains  an  exact  model  of  the  system  dynamics. 

The  analysis  of  how  the  error  covariance  behaves  when  the  gain  matrix 
is  computed  using  perturbed  values  of  the  F  matrix,  such  as  varying 
parameters  due  to  different  flight  conditions,  is  well  explained  in 
[Ref.  5].  Figure  3  is  a  block  diagram  of  the  system  model  and  Kalman 
Filter  with  the  system  dynamics  perturbed.  F*  is  the  perturbed  system 
dynamics,  while  K*  is  the  associated  gain  matrix  computed  for  the  Kalman 
Filter. 


SYSTEM  KALMAN  FILTER 


Figure  3.  System  Model  and  Kalman  Filter  with 
Perturbed  Dynamics 
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The  equation  for  the  estimate  is  given  by 


x  =  F*S  +  K*(z-Hx) 

The  error  in  the  estimate  is  given  by 


(13) 


x  =  (F*  -  K*H)x  +  aFx  -  Tw  +  K*v  (14) 

where 

AF  -  F*  -  F  (15) 

The  differential  equations  for  the  states  of  linear  system  driven  by 
white  gaussian  noise  now  become 


F*  -  K*H 
— <T 


Letting  x‘  be  the  augmented  state  vector 
The  covariance  matrix  of  x'  is  given  by 


]  M  [i]  *  he*] 

•  *■ 4  M 


E(x* 


(16) 


(17) 


where  one  defines  P  -  E(xxT),  V  -  E(xxT),  and  U  -  E(xxT).  P,  the  covar¬ 
iance  of  x,  is  the  quantity  of  interest.  The  error  sensitivity  equations 
are: 

P  =  (F*  -  K*H)  P  +  P(F*  -  K*H)T  +  AFV  +  VTAF  +  TQrT  +  K*RK*T  (18-a) 
V  =  FV  +  V(F*  -  K*H)T  +  UAFT  -  TQrT  (18-b) 
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and 


(18-c) 


0  =  FU  +  UFT  +  TQrT 

with  initial  conditions  P(0)  =  -V(0)  ~  U(0)  =  E(x(0)x(0)^).  When  the 
actual  system  dynamics  are  reproduced  in  the  filter,  F  =  F*  and  AF  =  0, 
and  equation  (18)  reduces  to  the  linear  Lyapunov  equation  of  equation 
(8). 

0.  MODAL  COORDINATES  TRANSFORMATION 

The  system  represented  by  equation  (1)  is  not  unique.  Consider  an 
alternate  linear  transformaton  of  the  states  described  in  references  [3] 
and  [6].  Let  x  -  T,  where  £  represents  the  transformation  of  the  states 
and  T  is  the  transformation  matrix  with  the  columns  formed  by  the  eigen¬ 
vectors  of  the  system  matrix  F  (for  a  complex  eigenvalue,  the  first 
column  is  the  real  part  and  the  second  is  the  imaginary  part  of  the 
eigenvector).  The  similarity  transformation  of  equation  (1)  is 

£  =  A£  +  Bw  (19-a) 

z  =  Cl  +  v  (19-b) 


where  A  =  T  ^  FT,  B  ~  T  V,  and  C  =  HT. 

A  case  of  particular  interest,  the  canonical  form,  results  when  the  A 
matrix  is  diagonal  (i.e.,  when  the  eigenvalues  of  the  F  matrix  appear  on 
the  diagonal).  This  canonical  form  is  more  informative  than  the  transfer 
function  method,  since  observability  and  control  ability  of  the  system  can 
be  obtained  by  inspection. 
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E.  SOLUTION  OF  THE  SKF  WITH  A  PRESCRIBED  DEGREE  OF  STABILITY 

The  constant  gain  Kalman  Filter  (SKF)  used  as  an  observer  will 
diverge  if  undisturbed,  neutrally  stable  (UNS)  modes  are  in  the  system 
model.  In  references  [3]  and  [7]  the  authors  tiscussed  the  destabili¬ 
zation  of  the  system  model  (1).  The  amount  of  destabilization  can  be 
varied  until  the  suboptimal  observer  formed  has  a  desired  degree  of 
stability.  The  method  of  [Ref.  3]  destabilizes  only  the  UNS  modes  in  the 
system  model  and  is  called  "modal  destabilization"  (MDS).  In  this  tech¬ 
nique  the  gains  of  the  filter  are  constrained  so  that 

ReCSj)  >  -a,  i  =  1,2, . ,n  (20) 

where  Re(S.)  indicates  the  "real"  part  of  (S.),  S^,...,Sn  are  the  eigen¬ 
values  of  the  filter,  i.e. ,  the  roots  of  equation  (9),  and  a  is  a  speci¬ 
fied  positive  number. 

The  original  system  model  is  destabilized  in  accordance  with  equation 
(21),  where  F1  is  the  destabilized  matrix  formed,  E  is  the  destabili¬ 
zation  matrix  (diagonal),  and  T  is  the  modal  transformation  matrix 
(eigenvector  matrix).  The  matrix  F'  is  used  to  calculate  the  suboptimal 
gains  of  the  filter. 

F1  =  F  +  TET-1  (21) 

This  MDS  approach  prevents  the  divergence  of  the  steady-state  Kalman 
Filter  in  a  system  with  UNS  model  while  causing  only  a  slight  reduction 
in  the  estimation  accuracy. 
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III.  DYNAMIC  AND  MEASUREMENT  SYSTEM  MODELS 


A.  REFERENCE  AXIS  SYSTEM 

The  Reference  Axis  System  of  a  missile  is  centered  at  its  center  of 
gravity  (c.g. )  and  fixed  on  the  missile  body  as  follows: 

X  axis,  the  roll  axis,  forward  from  the  c.g.  along  the  axis  of 
symmetry. 

Y  axis,  the  pitch  axis,  outward  to  the  right  from  the  c.g.  when 
viewing  the  missile  from  behind. 

Z  axis,  the  yaw  axis,  downward  from  the  c.g.  in  the  plane  of  sym¬ 
metry  to  form  a  right-handed  orthogonal  system  with  the 
other  two. 

Appendix  A  lists  the  symbols  defining  quantities  associated  with  the 
missile  illustrated  in  Figure  4  below  such  as  forces  and  moments,  linear 
and  angular  velocities,  and  moments  of  inertia. 


B.  MISSILE  EQUATIONS  OF  MOTION 


The  equations  of  motion  used  to  represent  the  missile  dynamics  used 
in  this  study  are  wall  defined  in  [Ref.  8].  A  linear  dynamical  model  of 
the  missile  based  on  the  rigid  body  approximation  is  appropriate. 

1 .  Longitudinal  Motion 

The  longitudinal  motions  of  a  missile  can  be  modeled  by  a  fifth- 
order  system  of  equation  (22),  where  the  state  variables  are  u,  velocity 
along  the  X  axis,  w,  velocity  along  the  Z  axis,  q,  pitch  rate,  9,  pitch 
angle  and  h,  altitude.  The  units  are:  u  and  w  in  10  ft/s,  q  in  0.01 
rad/s,  0  in  0.01  rad,  and  h  in  100  ft. 


"u“ 

“  Xu 

Xw 

0 

-g 

0** 
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Zu 

Zw 
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w 
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= 

Mu+fNZu 

Mw+MwZw 

Mq+MwV 

0 

0 

q 

0 

C 

0 

1 

0 

0 

0 

_h_ 

0 

-0.1 

0 

V 

0- 

A 

2.  Lateral  Motion 

The  lateral  motions  of  a  missile  are  modeled  by  the  fifth-order 
system  given  by  equation  (23),  where  the  state  variables  are:  p,  sideslip 
angle,  r,  yaw  rate,  p,  roll  rate,  <fr,  roll  angle,  and  tjj,  heading  angle. 
The  units  are:  p  in  rad,  r  in  rad/s,  p  in  rad/s,  9  in  rad,  and  i|i  in  rad. 
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C.  MODEL  DYNAMICS 

The  aerodynamic  data  used  in  this  paper  appears  in  Appendix  B. 
Except  for  the  addition  of  system  and  measurement  noise  parameters  for 
the  distance  input,  the  models  and  noise  dynamics  are  the  same  as  those 
of  [Ref.  3]. 

1 .  Longitudinal  Motion  Estimation 

The  main  disturbance  inputs  are  the  two  wind  velocities  ug  and  w^. 
Under  certain  flight  conditions,  the  turbulance  represented  by  the  fluc¬ 
tuating  parts  of  Ug  and  w^  are  colored  noise.  They  are  modeled  by  first- 
order  shaping  filters  with  white  gaussian  noise  inputs  as  shown  in 
equation  (10).  The  linear  model  that  results  is  given  by  equation  (24) 
[Ref.  4]. 
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The  numerical  data  for  the  longitudinal  dimensional  derivatives 
was  used  in  equation  (22).  The  resultant  model  is  represented  by  equa¬ 
tion  (25)  which  corresponds  to  the  state  vector  augmentation  of  equation 


(11). 

Scaling 

is  done 

with  u,  w 

,  u  ,  and 

wg 

in  units 

of  10  ft/s,  c 

in 

units  of  0.01 

rad/s,  8 

in  units 

of  0.01  rad, 

and  h  in 

units  of 

100 

ft. 

u 

-0.015 

0.004 

0 

-0.0322 

0 

-0.015 

0.004 

u 

w 

-0.074 

-0.806 

0.824 

0 

0 

-0.074 

-0.806 

w 

• 

q 

-0. 749 
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-1.344 

0 

0 

-0.749 

-10.7 

q 

0 

= 

0 

0 

1 

0 

0 

0 

0 

0 
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0 

-0.1 
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The  measurement  model  shown  by  equation  (26)  assumes  a  rate  gyro 


in  order  to  measure  z_  and  a  barometric  altimeter  to  measure  zk 

q  h 


u 

w 


2.  Lateral  Motion  Estimation 

The  main  disturbance  input  is  the  lateral  wind  v.  The  turbulence 
represented  by  the  fluctuating  part  of  v  is  the  colored  noise,  which  is 
also  nodeled  as  a  first-order  shaping  filter  with  white  gauss i an  noise 
input  as  given  by  equation  (10).  The  resulting  shaping  filter  taken  from 
[Ref.  3]  is  given  by  equation  (27). 

Pg  =  -O.8530g  +  0.853m  (27) 

where  0g  =  Vg/V. 

The  numerical  data  for  the  lateral  dimensional  derivatives  was 
applied  in  equation  (23)  to  obtain  equation  (28),  which  corresponds  to 
the  state  vector  augmentation  of  equation  (11). 
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The  measurement  model  given  by  equation  (29)  below  represents  the  case 
where  the  measurement  z  is  taken  with  a  roll-rate  gyro  and  the  measure¬ 
ment  z  obtained  fro#  a  magnetic  compas*. 


0  10  0 
0  0  0  1 


(29) 
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IV.  ANALYSIS 


A.  SIMULATION 

The  Sensitivity  Covariance  Program  developed  for  application  in  the 
work  of  [Ref.  4]  was  used  to  solve  the  error  sensitivity  equations  of 
equation  (18).  The  program  was  originally  developed  to  handle  a  set  of 
105  linear  differential  equations  for  the  longitudinal  case  and  78  for 
the  lateral.  The  program  was  revised  to  accommodate  136  linear  differ¬ 
ential  equations  in  the  longitudinal  case  and  101  in  the  lateral,  to 
allow  for  an  additional  state  augmentation  (i.e.,  the  distance  measure¬ 
ment  to  the  longitudinal  model).  The  outputs  of  these  programs  are  the 
time  matrices  and  rtns  estimate  errors,  the  square  roots  of  the  diagonal 
elements  of  the  P  matrices.  The  OPTSYS  program,  the  use  of  which  is 
described  by  [Ref.  9]  and  amplified  by  [Ref.  10],  was  applied  to  calcu¬ 
late  the  Kalman  Filter  gains  to  be  inserted  into  the  Sensitivity  Covar¬ 
iance  Program  to  find  the  estimate  errors  for  specific  system  parameter 
perturbations.  The  OPTSYS  program  was  also  used  to  destabilize  the 
systems  that  contained  UN5  modes  in  an  attempt  to  eliminate  filter 
divergence.  Copies  of  the  OPTSYS  and  the  Sensitivity  Covariance  programs 
follow  under  COMPUTER  PROGRAMS,  while  a  copy  of  [Ref.  10]  appears  in 
Appendix  C. 

B.  RESULTS 

As  the  problem  is  introduced,  the  results  are  presented  in  three 
parts:  (1)  to  verify  the  findings  of  [Ref.  3]  and  [Ref.  4]  with  the 
correct  implementation  of  the  dynamics  in  the  filter  parameters,  (2)  to 
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reproduce  the  findings  of  [Ref.  4]  for  the  longitudinal  motion  estimator 
with  incorrect  implementation  of  the  dynamics  in  the  Kalman  Filter,  and 
(3)  to  conduct  a  sensitivity  analysis  of  the  longitudinal  motion  esti¬ 
mator  after  adding  a  distance  measurement  and  associated  system  and 
measurement  noise  parameters  to  the  model  dynamics. 

1 .  Motion  Estimation  Analysis  for  Exact  Dynamics 

The  OPTSYS  program  was  used  with  input  data  representing  the 
actual  system  dynamics  for  both  the  longitudinal  and  lateral  cases  to 
obtain  the  following  results  which  are  the  same  as  those  of  [Ref.  4]  and 
essentially  the  same  as  those  of  [Ref.  3]. 
a.  Longitudinal  Case 
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b.  Lateral  Case 
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2.  Longitudinal  Motion  Estimation  Analysis 

The  OPTSYS  program  was  used  to  compute  a  new  K*  matrix  as  each 
parameter  of  the  F  matrix  was  individually  numerically  varied.  The 
Sensitivity  Covariance  Program  was  then  executed  utilizing  each  new  K* 
and  F*  matrix  pair  to  determine  the  rms  errors  for  each  individual  per¬ 
turbation. 

The  results  are  shown  in  Tables  1-8  and  are  identical  to  those  of 
[Ref.  4].  The  true  values  for  the  unperturbed  system  dynamics  parameters 
are  indicated  in  the  tables  by  an  asterisk.  A  discussion  of  the  results 
fallows: 
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The  dimensional  variation  of  the  X  force  with  forward  speed  u 
has  a  nominal  value  of  -0.015.  This  quantity  was  varied  in  a 
range  of  ±20515.  The  behavior  of  the  rms  estimate  errors  can  oe 
seen  in  Table  1.  The  tabulation  shows  that  the  numerical 
variation  of  the  Xu  derivative  does  not  cause  significant 
changes  in  the  nominal  values  of  the  rms  estimate  errors  of  the 
states  w,  q,  §,  u^,  and  w^.  The  states  u  and  h  appear  to  be 
slightly  effected,  but  not  enough  to  be  of  importance. 

The  dimensional  variation  of  the  X  force  with  downward  speed  w 
has  a  nominal  value  of  0.004.  Again,  a  numerical  variation  in 
a  range  of  ±2056  was  conducted.  The  behavior  of  the  rms  errors 
is  demonstrated  by  Table  2.  Comparing  these  values  with  the 
nominal  ones  reveals  that  changes  in  the  Xw  derivative  have 
essentially  no  effect  on  the  states  w,  q,  0,  u^,  and  wg,  while 
the  states  u  and  h  show  changes  too  small  to  consider  important. 

The  dimensional  variation  of  the  Z  force  caused  by  a  change  in 
the  forward  speed  u  has  a  nominal  value  of  -0.074.  The  design 
value  was  altered  in  a  range  of  ±20%  with  the  results  shown  in 
Table  3.  Evaluation  of  this  data  indicates  that  all  the  rms 
errors  show  some  sensitivity  except  for  that  of  q.  The  most 
significant  changes  occur  in  the  u,  §,  and  h  states.  The  large 
variation  in  u  can  be  important  in  terms  of  the  accuracy  in 
radial  position. 

The  dimensional  variation  of  the  Z  force  with  downward  speed  w 
has  a  nominal  value  of  -0.806.  The  results  for  this  case  with 
changes  in  Zw  over  a  range  of  ±20%  follow  in  Table  4.  They 
show  that  all  the  rms  estimate  errors  are  quite  sensitive  and 
any  variation  of  Zw  beyond  ±2%  can  be  considered  critical  and 
unacceptable. 
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TABLE  1.  RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  Xu  DERIVATIVE 


Xu 

u 

ft/s 

— : — 
w 

ft/s 

q 

deg/s 

0 

deg 

h 

ft 

ug 

ft/s 

wg 

ft/s 

-0.018 

2.096 

5.102 

0.416 

0.317 

8.248 

4.776 

5.701 

-0.0165 

2.094 

5.102 

0.416 

0.317 

8.246 

4.776 

5.701 

-0.01575 

2.091 

5. 102 

0.416 

0.317 

8.240 

4.776 

5.701 

-0.015  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.01425 

2.088 

5.103 

0.416 

0.317 

8.260 

4.775 

5.701 

-0.0135 

2.089 

5.103 

0.416 

0.317 

8.280 

4.775 

5.701 

-0.012 

2.092 

5.103 

0.416 

0.317 

8.340 

4.775 

5.701 

TABLE  2.  RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 

WITH  VARIATION  IN  X  DERIVATIVE 
w 


Xw 

u 

ft/s 

w 

ft/s 

q 

deg/s 

0 

deg 

h 

ft 

ug 

ft/s 

wg 

ft/s 

0.0048 

2.070 

5.102 

0.416 

0.317 

8.319 

4.776 

5.701 

0.0044 

2.080 

5.102 

0.416 

0.317 

8.282 

4.776 

5.701 

0.0042 

2.086 

5.102 

0.416 

0.317 

8.257 

4.776 

5.701 

0.004  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

0.0038 

2.100 

5.102 

0.416 

0.317 

8.223 

4.776 

5.701 

0.0036 

2.103 

5.102 

0.416 

0.316 

8.215 

4.776 

5.701 

0.0032 

2.110 

5.101 

0.416 

0.316 

8.180 

4.776 

5.701 

TABLE  3.  RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  Z DERIVATIVE 


Zu 

u 

ft/s 

w 

ft/s 

q 

deg/s 

9 

deg 

h 

ft 

ug 

ft/s 

wg 

ft/s 

-0.0888 

1.885 

5.106 

0.416 

0.322 

9.310 

4.776 

5.707 

-0.0814 

1.974 

5.104 

0.416 

0.319 

8.808 

4.775 

5.703 

-0.0777 

2.026 

5.194 

0.416 

0.318 

8.579 

4.775 

5.702 

-0.740  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.0703 

2.122 

5.100 

0.416 

0.315 

7.800 

4.777 

5.700 

-0. 0666 

2.270 

5.095 

0.416 

0.313 

7.101 

4.777 

5.697 

-0.0592 

2.32 

5.094 

0.416 

0.311 

7.000 

4.778 

5.695 

TABLE  4.  RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  Zw  DERIVATIVE 


Zw 

u 

ft/s 

w 

ft/s 

q 

deg/s 

9 

deg 

h 

ft 

ug 

ft/s 

wg 

ft/s 

-0.9612 

30.08 

6.172 

0.486 

0.440 

17.97 

4.778 

7. 138 

-0.8866 

11.80 

5.810 

0.428 

0.415 

33.50 

4.785 

5.957 

-0.8463 

5.345 

5.259 

0.421 

0.400 

22. 776 

4.779 

5.737 

-0.806  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.7657 

2.668 

5.032 

0.412 

0.219 

16.903 

4.772 

5.710 

-0.7256 

3.188 

5.035 

0.407 

0.200 

21.026 

4.760 

5.746 

-0.665 

3.260 

5.065 

0.406 

0.185 

23.000 

4.767 

5.794 
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Mu>  The  dimensional  variation  of  the  M  moment  caused  by  a  change  in 
the  forward  speed  u  has  a  nominal  value  of  -0.000786.  From 
Table  5,  one  notes  that  the  rms  errors  for  tha  states  q,  ug, 
and  Wg  are  not  effected  by  a  variation  in  My  of  ±20%,  but 
significant  changes  are  seen  when  Mu  is  varied  more  than  ±10% 
in  the  errors  of  states  u,  w,  8,  and  h. 

Mw-  The  dimensional  variation  of  the  M  moment  with  speed  w  has  a 
nominal  value  of  -0.0111.  The  results  of  a  numerical  variation 
in  a  range  of  ±10%  can  be  seen  in  Table  6.  Since  any  alter¬ 
ation  in  the  true  value  of  Mw  has  a  strong  effect  on  al  .  the 
rms  estimate  errors,  this  derivative  can  be  considered  the  most 
critical  in  the  longitudinal  motion  estimation  case. 

Mg.  The  dimensional  variation  of  the  pitching  moment  with  pitch 
rate  q  has  a  nominal  value  of  -0.924.  The  results  of  Table  7 
on  the  rms  estimate  errors  for  a  ±20%  change  in  show  that 
the  sensitivity  to  variations  in  this  parameter  is  minimal  for 
all  states. 

M^  The  dimensional  variation  of  the  pitching  moment  with  the  rate 
of  change  of  the  downward  speed  w  has  a  nominal  value  of 
-0.00051.  Table  8  contains  the  rms  errors  data  obtained  by 

mitering  M^  ±20%  from  its  nominal  value.  All  the  errors  show  a 
degree  of  sensitivity  and  the  variation  of  errors  is  signifi¬ 
cant  when  M^  is  changed  by  more  than  ±2%. 

Since  plots  of  the  rms  estimate  errors  versus  changes  in  the 
particular  dimensional  derivatives  for  data  identical  to  that  of  Tables 
1-8  appears  in  [Ref.  4]  in  Figures  35-40,  they  will  not  be  repeated  in 
this  work.  A  summary  of  the  relative  sensitivity  of  the  rms  estimate 
errors  to  changes  in  the  individual  dimensional  derivatives  follows  in 
Table  9. 
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TABLE  5.  RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  Mu  DERIVATIVE 


Mu 

u 

ft/s 

w 

ft/s 

3 

deg/s 

§ 

deg 

D 

“g 

ft/s 

wg 

ft/s 

-0.000943 

2.234 

5.061 

0.416 

0.305 

6.230 

4.775 

5.689 

-0.000865 

2.115 

5.089 

0.416 

0.310 

6.640 

4.775 

5.695 

-0.000825 

2.104 

5.094 

0.416 

lisa 

7.531 

4.776 

5.698 

-0. 000786* 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.000747 

1.993 

5.105 

0.416 

0.318 

8.595 

4.777 

5.703 

-0.000707 

1.866 

5.108 

0.416 

0.319 

8.832 

4.779 

5.705 

-0.000629 

1.566 

5.111 

0.416 

0.322 
_ 1 

9.178 

4.783 

5.708 

TABLE  6.  RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  Mw  DERIVATIVE 


Mw 

u 

ft/s 

w 

ft/s 

q 

deg/s 

e 

deg 

a 

ug 

ft/s 

wg 

ft/s 

-0.01165 

18.43 

8.350 

0.502 

0.455 

29.870 

4.778 

5.695 

-0.0113 

5.163 

5.142 

0.433 

0.373 

17.790 

4.778 

5.694 

-0.0112 

3.110 

5.113 

0.418 

i 

0.321 

10.427 

4.777 

5.699  i 

-0.0111  * 

2.090 

51.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.0109 

5.206 

5.018 

0.419  1 

0.325 

16.650 

4.776 

5.700 

-0.01055 

13.652 

5.342 

0.447 

0.430 

J 

12.204 

4.776 

5.918 

-0.00999 

19.13 

18.95 

0.475 

0.704 

68.98 

4.756 

6.102 
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TABLE  7.  RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  Mq  DERIVATIVE 


% 

u 

ft/s 

w 

ft/s 

q 

deg/s 

B 

n 

ug 

ft/s 

wg 

ft/s 

-1.109 

2.091 

5.102 

0.416 

0.316 

8.230 

4.776 

5.701 

-1.016 

2.091 

5.102 

0.416 

0.316 

8.234 

4.776 

5.701 

-0.970 

2.091 

5.102 

0.416 

0.317 

8.236 

4.776 

5.701 

-0.924  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.880 

2.090 

5.102 

0.416 

0.316 

8.244 

4. 776 

5.701 

-0.832 

2.089 

5.102 

0.416 

0.316 

8.236 

4.776 

5.701 

-0.7392 

2.089 

5.102 

0.416 

j 

0.316 

8.232 

4.776 

5.701 

TABLE  8.  RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  DERIVATIVE 


M- 

w 

u 

ft/s 

w 

ft/s 

RH 

i 

0 

deg 

H 

ug 

ft/s 

"g 

ft/s 

-0.00061 

2.610 

5.053 

0.417 

0.273 

10.665 

4.775 

5.688 

-0.00056 

2.476 

5.089 

0.417 

0.295 

6.301 

4.776 

5.703 

-0.00053 

2.398 

5.091 

0.417 

0.304 

4.150 

4.776 

5.698 

-0.00051* 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0. 00049 

2.491 

5.108 

0.416 

0.322 

9.757 

4.776 

5.702 

-0. 00046 

2.976 

5.118 

0.415 

0.332 

12.067 

4.776 

5.703 

-0.00041 

3.570 

5.124 

0.415 

0.340 

13.536 

4.776 

5.7C3 

TABLE  9. 


RELATIVE  SENSITIVITY  OF  THE  RMS  ESTIMATE  ERRORS 
TO  CHANGES  IN  DERIVATIVES 


DERIVATIVE 

NS 

RS 

vs 

Xu 

X 

X 

w 

X 

Zu 

X 

Zw 

X 

X 

Mw 

X 

X 

X 

NS  =  Not  sensitive 

RS  =  Relatively  sensitive 

VS  =  Very  sensitive 


35 


3.  Analysis  of  Longitudinal  Motion  Estimation  After  Augmentation 
Including  the  distance  measurement  to  the  system  required  state 
augmentation  cf  the  system  and  measurement  models  as  demonstrated  by 
equations  (30)  and  (31)  that  follow: 


u 

-0.015 

0.004 

0 

-0.0322 

0 

-0.015 

0.004 

w 

-0.074 

-  0.806 

0.824 

0 

0 

-0.074 

-  0.806 

• 

q 

-0.749 

-10.7 

-1.344 

0 

0 

-0.749 

-10.7 

e 

s 

0 

0 

1.0 

0 

0 

0 

0 

h 

0 

-  0.1 

0 

0.824 

0 

0 

0 

• 

ug 

0 

0 

0 

0 

0 

-0.413 

0 

"g 

0 

0 

0 

0 

0 

0 

-0.853 

_d  _ 

1.0 

m 

0 

0 

0 

0 

0 

0 

"o 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

'V 

+ 

0 

0 

0 

0.413 

0 

0 

-Md- 

0 

0.353 

0 

0 

0 

1.0 

(30) 
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and 


0 

0 

0 


1 

0 

0 


0 

0 

0 


0 

0 

0 


0 

0 

0 


1  0  0“ 

V  ~ 

q 

0  1  0 

vh 

0  0  1 

_wd 

(31) 


In  these  equations  the  state  variables  are  u,  velocity  along  the  X  axis, 
w,  velocity  along  the  Z  axis,  q,  pitch  rate,  0,  pitch  angle,  h,  altitude 
and  d,  distance  traveled  along  the  X  axis.  The  units  are:  u  and  w  in 
10  ft/s,  q  in  0.01  rad/s,  0  in  0.01  rad,  h  in  100  ft,  and  d  in  10  ft. 

The  0PTSYS  program,  when  executed  with  the  data  from  equations 
(30)  and  (31)  above  and  the  standard  deviation  values  from  Appendix  B, 
yielded  the  following: 

filter  gain  matrix  K 
0.0592  0.0570  0.0014 

0.2646  -0.1611  -0.0007 

3.5168  0.0404  0.0002 

0.0011  -0.0810  -0.0007 

0.0135  0.1353  0.0001 

-0.0114  0.0356  -0.0002 

-1.2876  0.1283  0.0005 

0.0469  0.0561  1.0014 
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filter  eigenvalues 

-3.103  ±  4.1103 

-1.C00 

-0.4146 

-0.3152 

-0.0485  ±  0.0548 
-0.0551 

rms  estimate  errors 


2.090 

ft/s 

§  = 

0.316 

deg 

5.102 

ft/s 

h  = 

8.225 

ft 

0.416 

deg/s 

“g  " 

4.776 

ft/s 

5.701 

ft/s 

d  = 

38. 730 

ft 

9 

The  next  step  in  the  analysis  was  to  perturb  each  directional 
derivative  independently  by  specific  amounts  from  its  nominal  value  and 
to  observe  the  effect  on  the  ras  estimate  errors.  This  process  was 
carried  out  for  all  eight  directional  derivatives  and  the  response  was 
the  same  for  each  case  --  even  a  slight  perturbation  of  -0.1%  of  any 
directional  derivative  from  its  nominal  value  caused  the  rms  estimate 
errors  to  increase  without  bound.  This  behavior  indicated  that  any 
incorrect  implementation  of  dynamics  in  the  new  system  formed  by  the 
augmentation  of  the  distance  measurement  would  cause  i  stability  and  the 
Kalman  Filter  to  diverge. 

Several  system  parameters  were  individually  modified  and  the 
analysis  repeated  in  hopes  of  finding  a  stable  system  for  which  the 
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Hainan  Filter  converged.  The  coefficient  for  the  distance  tern  in  the 
process  noise  distribution  matrix  was  varied  from  0.01-5.0,  the  power 
spectral  density  process  noise  entry  for  distance  was  changed  in  a  range 
of  1.105-30.0,  and  the  distance  tern  for  the  power  spectral  density 
measurement  noise  adjusted  over  a  range  of  0.03-30.0.  None  of  these 
trials  led  to  a  stable  system. 

A  modal  analysis  was  also  performed  using  the  open  loop  eigen¬ 
values  from  the  OPTSYS  output  listing.  The  system  of  equations  (30)  and 
(31)  when  transformed  into  modal  coordinates  give  equations  (32)  and  (33) 
below: 


1 — 
UJ 

0 

0 

0 

0 

0 

0 

0 

o 

id 

0 

0 

0 

0 

0 

0 

0 

0 

*d 

*.l 

0 

0 

-1.076 

2.957 

0 

0 

0 

0 

ls1 

^s2 

= 

0 

0 

2.957 

-1.076 

0 

0 

0 

0 

^s2 

^pl 

0 

0 

0 

0 

-0.006 

0.024 

0 

0 

£pl 

i* 

0 

0 

0 

0 

0.024 

-0.006 

0 

0 

Cp2 

iug 

0 

0 

0 

0 

0 

0 

-0.413 

0 

*\» 

Ihl 

0 

0 

0 

0 

0 

0 

0 

-0.853 

0 

0. 1 

-1.0 

0 

-0.028 

-0.088 

-o.no 

-3.153 

1.027 

-0.048 

-0.210 

1.467 

0.420 

0 

Lo 

1.836 

0 

1.0 

0 

0 

0 

0 

0 

0 


lMd 


(32) 
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zq 

0 

0 

1.0 

0 

-0.0001 

0.0005 

0.0548 

0.4802 

zh 

= 

1.0 

0 

0.0016 

0.0016 

-0.0156 

-0.0612 

0.0082 

-0.0025 

_zd_ 

0 

1.0 

-0.0008 

-0.0004 

1.0 

0 

-0.0657 

-0.0252 

loo 

v^ 

g 

+ 

o 

o 

vh 

0  0  1 

,vd_ 

^sl 

^s2 

*p1 

Cp2 


(33) 


Consistent  with  the  discussion  in  [Ref.  3],  inspection  of  equa¬ 
tion  (32)  revealed  that  the  energy  mode  and  the  distance  mode  4d.  were 
neutrally  stable  (i.e.  ,  eigenvalues  =  0).  Inspection  of  equation  (33) 
showed  that  was  unobservable  with  z^  and  zd  and  that  was  unobserv¬ 
able  with  and  zh.  Equation  (32)  also  disclosed  that  £E  was  undis¬ 
turbed  by  Ug  and  d,  and  that  £d  was  undisturbed  by  wg.  Tnerefore,  de- 
stabilization  was  conducted  in  an  attempt  to  prevent  filter  divergence. 
Both  total  and  modal  destabilization  described  earlier  in  this  work  and 
in  [Ref.  3]  were  performed  in  amounts  of  0.040  and  1.0  using  the  OPTSYS 
program.  The  filter  gains  computed  for  the  destabilized  system  were  then 
executed  in  the  Sensitivity  Covariance  Program  with  each  of  the  modified 
parameter  combinations  discussed  earlier.  Without  exception,  the  rms 
estimation  errors  increased  without  bound  when  the  least  sensitive  dimen¬ 
sional  derivative  Xu  was  perturbed  by  as  little  as  ±1%. 
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V.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  COHCLUSIONS 

The  conclusions  reached  were  based  on  the  results  obtained  and  will 
therefore  be  presented  in  three  parts. 

1 .  Motion  Estimation  Analysis  for  Exact  Dynamics 

The  data  in  the  results  is  in  agreement  with  that  of  both 
[Ref.  3]  and  [Ref.  4].  It  shows  that  both  the  Kalman  Filters  for  initial 
longitudinal  and  lateral  cases  are  stable  when  the  true  values  for  the 
system  dynamics  are  implemented. 

2.  Longitudinal  Motion  Estimation  Analysis 

The  results  from  Tables  1-8  and  summarized  in  Table  9  are  con¬ 
sistent  with  those  of  [Ref.  4].  The  stability  derivatives  Zw  and  Mw 
cause  the  strongest  changes  in  the  rms  estimate  errors  when  they  are 
varied.  Basically,  Zw  and  Mw  must  be  quite  accurately  reproduced  in  the 
filter  to  prevent  divergence.  Changes  in  the  stability  derivatives  Zu> 
Mu,  and  M,*  reflect  intermediate  variations  in  nearly  all  the  rms  estimate 
errors.  For  the  model  considered  a  tolerance  of  more  than  ±5%  affects 

the  accuracy  in  the  radial  position  since  large  variations  in  u  occur.  A 

tollerance  of  perhaps  ±20%  can  be  accepted  in  the  dimensional  derivatives 

X  ,  X  ,  and  M  for  tnis  model  since  no  important  effect  is  noted  in  the 
u  w  q 

rms  errors  over  that  range. 

3.  Analysis  of  Longitudinal  Motion  Estimation  After  Augmentation 

From  the  results  presented  earlier  for  the  new  system  model  fomed 

by  the  augmentation  of  a  distance  measurement  and  associated  process  and 
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measurement  noise  parameters,  it  is  apparen*  that  the  corresponding 
Kalman  Filter  will  diverge  for  even  a  slight  variation  in  any  of  the 
dimensional  derivatives  from  their  nominal  values.  Even  the  Kalman 
Filter  developed  by  system  destabilization  proved  to  be  unstable  with  the 
parameters  used. 

B.  RECOMMENDATIONS 

Further  analysis  of  the  augmented  system  including  the  distance  or 
position  estimation  is  desirable.  Perhaps  a  more  in-depth  study  of  the 
measurement  parameter  scaling  would  enable  the  development  of  a  stable 
Kalman  Filter  for  at  least  a  destabilized  system. 


VI.  SUMMARY 


The  sensitivity  analyses  performed  in  this  work  have  revealed  the 
importance  of  accuracy  in  determining  system  dynamics  utilized  in  formu¬ 
lating  the  model  for  the  Kalman  Filter.  The  relative  sensitivity  of  the 
rms  estimation  errors  to  variance  in  each  of  the  particular  dimensional 
derivatives  is  shown  in  Table  9  for  the  Longitudinal  Motion  Estimator. 

The  longitudinal  system  augmented  with  the  distance  measurement 
developed  appears  to  be  extremely  sensitive  to  variations  in  all  the 
dimensional  derivatives.  Further  analysis  of  the  model  developed  is 
suggested. 


43 


APPENOIX  A 
LIST  OF  SYMBOLS 


Regular  Symbols 
A 
B 
C 
D 
d 
E 
F 
f 
F‘ 

g 

H 

H 

h 

INS 

K 

L 

M 

MOS 

N 

n 

P 

P 

q 

q 

R 

r 

S 


Definition 

Modal  transformation  of  F  matrix 

Modal  transformation  of  r  matrix 

Modal  transformation  of  H  matrix 

Dutch  roll  mode 

Distance  traveled  along  the  X  axis 
Destabilization  matrix 
System  dynamics  matrix 
Subscript  for  filter 
Destabilized  matrix 
Subscript  for  wind  speed 
Measurement  matrix 
Heading  Mode 
Altitude 

Inertial  Navigation  System 

Kalman  Filter  gain  matrix 

Rolling  moment  (about  X  axis) 

Pitching  moment  (about  Y  axis) 

Modal  destabilization 

Yawing  moment  (about  Z  axis) 

Non-white  gaussian  noise 

Covariance  propagation  of  the  estimate 
error  matrix 

Perturbed  roll  rate 

Covariance  matrix  of  w 

Perturbed  pitch  rate 

Covariance  matrix  of  v 

Perturbed  yaw  rate 

Spiral  mode 
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Regular  Symbols 
SKF 
T 

UNS 

u 

V 
v 
w 

wg 

x 

x 

* 

X 

“V 

X 

Y 
z 
Z 


Definition 
Steady* State  Kalman  Filter 
Transformation  matrix 
Undisturbed  neutrally  stable 
Perturbed  forward  speed  (along  X  axis) 
Forward  velocity 
Perturbed  side  velocity 
Driving  white  gaussian  noise 
Perturbed  downward  velocity 
Reference  axis 
State  vector  of  the  system 
State  estimate  vector 
Estimate  error  vector 
Reference  axis 
Measurement  vector 
Reference  Axis 


Greek  Symbols 

<1> 

0 

<0 

P 

r 

a 

a 

i 

X 


Definition 

Heading  angle 

Perturbed  pitch  attitude  angle 
Perturbed  bank  (roll)  angle 
Sideslip  angle 
Driving  noise  matrix 
Eigenvalue  constrain 
Standard  deviation 
Transformed  state  vector 
Time 
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APPENDIX  B 

AERODYNAMIC  DATA  AND  PROBABILISTIC  INFORMATION 
v  =  820  ft/s 


1 .  Longitudinal  Model 

a.  Dimensional  Derivatives 

Xu  =  0.015  1/s 

Xw  =  0.004  1/s 

Zu  =  -0.074  1/s 

Zw  =  -0.0806  1/s 

Mu  =  -0.0786  1/s-ft 

M W  =  -0.0111  1/s-ft 

Mq  =  -0.924  1/s-rad 

Mw  =  -0.00051  1/ft 

b.  Distrubance  Noise  Standard  Deviation 
au  =  ow  =  1.105  1/s  (10  ft/s)2 

<rx  =  30.0  1/s  (10  ft/s)2 

(7  ft/s  rms  gust  with  a  930-ft  correlation  distance). 

c.  Observation  Noise  Standard  Deviation 
a  =0.15  s  (0.01  rad/s)2 

oh  =  0.05  s  (100  ft)2 
od  =  30.0  s  (10  ft)2 
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Lateral  Models 


a.  Dimensional  Derivatives 
Yy  =  -0.0858  1/s 

Np  =  2.14  1/s 

N'  =  -0.228  1/s 
r 

N'  =  -0.0204  1/s 
P 

Li  =  -4.41  1/s2 

p 

L'  =  0.334  1/s 
r 

L'  =  -1.181  1/s 

P 

b.  Disturbance  Noise  Standard  Deviation 
a  -  1.63  x  10“4  1/s 

(7  ft/s  rms  gust  with  a  930- ft  correlation  distance) 

i 

c.  Observation  Noise  Standard  Deviation 
op  =  1.5  x  10"5  s 

cj^  =  1.5  x  10~5  1/s 
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APPENDIX  C 

AN  AID  TO  USING  OPTSYS  ATNPS 


I.  INTRODUCTION 

One  of  the  tasks  involved  in  my  thesis  work  at  the  Naval  Postgraduate 
School  (NPS)  was  to  verify  some  of  the  data  of  reference  [1]  which  in¬ 
vestigated  the  sensitivity  of  the  Steady-State  Kalman  Filters  as  lateral 
and  longitudinal  estimators  in  Strapdown  Inertial  Navigation  Systems 
(INS).  One  of  the  recurring,  essential  calculations  was  for  the  steady- 
state  gains  of  each  system  model  considered.  Fortunately,  the  OPTSYS 
computer  program  was  available  in  Fortran  at  the  computer  center  to  help 
perform  this  enormous  job.  The  use  of  the  OPTSYS  program  was  covered  by 
reference  [2],  but  not  in  adequate  detail  for  easy  application.  After 
much  trial -and-error,  frustration,  attempted  decoding  with  the  assistance 
of  the  computer  center  staff,  and  prayer,  and  at  the  expense  of  many 
man-hours  of  time,  our  Lord  enabled  me  to  properly  fill  out  and  order  the 
data  cards  for  a  particular  modeled  system  and  obtain  the  expected 
results  upon  execution  of  the  program.  Since  Professor  Collins  has 
several  other  students  in  need  of  a  users  working  knowledge  of  the  OPTSYS 
program  and  anyone  using  Kalman  Filters  can  benefit  as  well,  I  am  writing 
a  more  detailed  description  of  how  to  correctly  input  data  by  discussing 
a  specific  example.  The  intent  of  this  paper  is  to  supplement  the 
guidance  of  reference  [2]  and  further  facilitate  research  at  NPS. 


II.  MODEL  AND  ESTIMATION 


Consider  the  linear  time- invariant  system  given  by 

x  =  Fx  +  Tw 
z  =  Hx  +  v 

where  x  represents  the  states  of  the  system;  z  is  the  measurement  vector; 
F  is  the  system  matrix;  r  is  the  driving  noise  coefficient  matrix;  H  is 
the  measurement  scaling  matrix;  and  w  and  v  are  independent,  zero-mean, 
white  gaussian  noise  processes  with  covariance  matrices  Q  and  R, 
respectively. 

A  continuous  time  Kalman  Filter  for  this  system  is  described  by 

x  =  Fx  +  K(z  -  Hx) 

where  x  is  the  state  estimate  and  K  is  the  matrix  of  the  steady-state 
gains  of  the  Kalman  Filter.  The  implementation  of  the  System  Model  and 
the  Kalman  Filter  are  shown  below  in  Figure  C-l  [Ref.  1]. 
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MATHEMATICAL  MODEL 
SYSTEM 


MEASUREMENT 


KALMAN 


Figure  C-l .  System  Model  and  Kalman  Filter 


III.  AN  EXAMPLE  OF  LONGITUDINAL  MOTION  ESTIMATION 


After  state  vector  augmentation,  the  resultant  model  of  longitudinal 
motion  of  an  aircraft  of  the  form  x  *  Fx  +  Pw  is 


mm  ^ 
• 

u 

*0.015 

0.004 

0 

* 

w 

-0.074 

-0.806 

0.824 

* 

q 

-0. 749 

-10.7 

-1.344 

0 

s 

0 

0 

1 

h 

0 

-0.1 

0 

• 

ug 

0 

0 

0 

;g_ 

0 

0 

0 

0.0322 

0 

-0.015 

0.004 

u 

0 

0 

-0.074 

-0.806 

w 

0 

0 

-0. 749 

-10.7 

q 

1 

0 

0 

0 

0 

0.0824 

0 

0 

0 

h 

0 

0 

-0.413 

0 

u3 

0 

0 

0 

-0.853 

h 

0 

0 

0 


+ 


0 


0 

0 

0 

0 


0  0 


0.413  0 

0  0.853 


where  the  units  are  scaled  such  that  u,  w,  u  ,  and  w  must  be  multiplied 

9  9 

by  10  to  give  feet  per  second,  q  by  0.01  to  give  radians  per  second,  0  by 
0.01  to  give  radians,  and  h  by  100  to  give  units  of  feet  [Ref.  1]. 
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The  corresponding  measurement  aadel  la  the  fora  z  =  Hx  +  Iv  is  given 


by 


(26) 


For  this  model  Qu  =  ^  =  1-105  (10  ft/s)2/s,  R  =  0.15  (0.01  rad/s)2  and 

UW  H 

Rh  =  0.05  (100  ft)2  s  [Ref.  3]. 
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IV.  APPLYING  OPTSYS  TO  THE  EXAMPLE 


The  essential  input  data  that  will  enable  OPTSYS  to  calculate  the 
steady-state  gains  of  the  Kalman  Filter  and  many  other  parameters  out¬ 
lined  in  [Ref.  2]  follows  on  page  55.  The  input  data  and  control  cards 
are  described  in  the  paragraphs  below. 

Card  1  -  The  17  entries  in  every  other  column  from  column  2  through 
column  34  essentially  tell  OPTSYS  what  to  compute.  See  [Ref.  2]  for  more 
details. 

Card  2  -  The  5  entries  in  every  third  column  from  3  through  15  de¬ 
scribe  the  system  being  modeled  to  OPTSYS.  The  first  entry  tells  the 
number  of  states  or  order  of  the  system-7  since  there  are  seven  rows  in 
the  F  matrix.  The  second  entry  gives  the  number  of  control s-0  since  u=0. 
The  third  entry  tells  that  we  have  2  measurements,  while  the  fourth  entry 
shows  that  two  process  noise  sources  exist.  The  fifth  entry  is  always 
zero  when  filter  synthesis  is  done.  See  [Ref.  2]  if  regulator  synthesis 
only  is  desired. 

Cards  3-16  -  These  cards  contain  the  F  matrix.  The  first  six  entries 
of  each  row  go  on  one  card  with  12  columns  for  each  entry-1- 12,  13-24, 
60-72.  The  seventh  entry  for  each  row  is  placed  in  columns  1-12  of 
a  continuation  ccrd  that  immediately  follows  the  card  with  the  first  six 
entries  of  the  row.  Note  that  if  our  example  system  were  6x6,  the  F 
matrix  would  only  take  up  cards  3-8. 


S, 

» 
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The  next  three  cards,  17-19  in  our  example,  contain  the  H  matrix. 
Note  that  this  matrix  is  also  entered  on  the  cards  by  rows,  but  consecu¬ 
tively  with  an  entry  in  every  12  columns  with  6  entries  per  card  as  long 
as  unused  row  elements  remain!  Thus  the  first  entry  of  row  2  of  the  H 
matrix  appears  in  columns  13-24  of  card  18. 

The  next  three  cards,  20-22,  hold  the  r  matrix.  This  matrix  is  also 
entered  consecutively  by  rows  with  an  entry  in  the  first  14  groups  of  12 
columns  on  the  cards! 

The  next  to  the  last  card  gives  the  Q  matrix.  Note  that  this  card 
has  only  the  diagonal  terms  of  the  matrix  in  columns  1-12  and  13-24.  See 
[Ref.  2]  for  matrices  with  non-diagonal  terms. 

The  last  card  is  for  the  R  matrix  and  also  has  diagonal  entries  in 
columns  1-12  and  13-24.  Again  refer  to  [Ref.  2]  if  non-diagonal  terms 
exist. 

This  supplement  will  be  effective  until  the  OPTSYS  program  is 
re-coded  in  WATFIV  language.  Its  usage  should  greatly  improve  the 
efficienty  and  morale  of  those  using  the  OPTSYS  program  on  file  at  NPS 
Computer  Center. 
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COMPUTER  OUTPUTS 


C/OPTETS1  JOB  (1480,0417)  POTTES  G.G.' 
C/  E I  EC  PCRTCLG 
C/ST S I”  DC  * 


«-JCr  1 3  21  ,F.ES  (32)  . 

OHY(9,32(,HU{8.32, 

EOUIVALtNCE  IK1  1  (1.  1)  ,GN  <1,  1|  I.  (1111(1,1), i 

«  IDSTAE.IDEaOG.ISET,  IsSc.I PSD,  ITU,  IN  ORB 


*  d/.1)  .«*  o.ii  i . 

TF2.irP3,rFDPW,IE. 


4 

101 


74  3  2 


DATA  STAB/"'1/ 


FOR  EAT  (4012) 

READ (5.4, EHD-100)  IOL , TO, IN Q, IR . ISS, I M, 
>  IDSTAB.IDESOG.ISIT.IPSO ,ITU, IRORB 
HEAD(D,  7432)  MS,  NC,  MO  B,  NO,  I  REG 


ITF1,ITF2,ITF3,IFDFM,I£, 


C  tf  R  ,  D,  FfaGC,  F  BGE , 
GV.HY.MJ, 


WHITE  (6.4000)  NS,NC,NOB,  N3 

4000  FOEBATP  1*  .2*,'  ORDER  OF  SYSTEM  »*  ,  1 3,  // ,2X.  'RUBBER  OF  CONTROLS  »• 

1  ,  I3,//,2X,' NUMBER  OF  OBSERVATIONS  -',13,//, 2X, 

2  'Rubber  of  process  noise  sources  *',i3,//( 

M2- 29  MS 

CALL  INKER  (MS  ,  MC,  MO 9,  MG,  M2  ,  ACL,  B,  3A ,  C T ,  CR ,  CO.CWI ,  C WR 
0G, GAB, CB.CM,  HO,  ol,D2,  i'BO.HH  ,  RC,  0,SC,MF,  WI,  Nil,  wul  .  X, 

FWROSB, WNfiRBI, DESTAB, A A,B B,  C ft , JC ?, RES,  AY, BB.CC.CP.GW, 

«*  CSTOREI 

99  READ(5.5.EXD«100)  STA 
5  FOR  HA  T  I  A2) 

ir(  STAR.  EQ.  STA)  GOTO  10  1 
GOTO  99 
100  STOP 
END 

SUBROUTINE  SETUP  (3A  ,G  ,GAB,  MS,  MC,  NGI 
METURM 
END 

SUBFOUTIBE 

f  INNER  (NS. SC.  N0.NG.N2,  ACt.B  ,  BA.  Cl  .  CR.CQ.  CB  I,  CMF,  D ,  F 

*0,J  AN.CB.CN.HO.DI  ,  D2,  PRO  ,  RB  ,  RC,  Q.  SC  .  M  8.  W  t  .  3 1 1  ,  N2  1  ,X  , 
^MN3RB,MN5rHI,  OSSTAB,  AA,£.B,CS,j5P,?!S,  A Y  ,  B6  ,CC,  Cr  .  Gif,  G V  , 

9  DSTOrE^ 


FBGC.FBCE, 

MT.Ht), 


IBP  LI  Cl 
OrBFNSIOH 
"CWI 


RB( 


LICIT  3EAL"B  (A-H.O-El 

FNSION  ACL  (MS  tMS|  ,  blllC.  NCI  ,B  A  (MS,  NS  I  ,  Cl  (MSI  ,TK  (MS)  .COINS,  MS)  , 
(NSI.CHRINSI  ,  F  3GC  (  NO,  MS)  ,pDOEjNS,MOj  ,G(NS.NSt  .GRINS, MS)  , 
c  PRO  (NS  .MSI  .  Sc  (NO,  NO)  ,3C  (4S.N$|  ,#R  (M2>  ,  Vt  (N2  ,N1  1 
"(NS.NS)  ,M21  (NS, MS)  «X(N2,N2l  .GAINS. MS)  ,  HO  INO  .  Ms)  ,  5 )  (M2)  ,D2  IN2)  ,R.' 

9  2  ,B  2)  ,Q  (  NG  ,  NG)  ,  0<MO,NC)  ,3A  B  INS  ,H«)  ,  M  NORM  I  NS  ,  NSf  ,  W  NORM  I  (NS.NSJ 
"  ,  DISTABjfiS)  ,AA  (NS.NS  .  SB  (NS.MC)  .CRINO.NS)  ,  JCF(N21  ,  FES  (M2)  ,  A  V  (NO 

"  ,CB  (If  2)  ,CC  { N2)  ,CP  (MS)  ,  2  U  (m2  ,  MCI  ,  ,  V  (N  2  ,  NO)  ,HT  (  MO  ,  N2)  ,  HU  (MC  ,  M2) 
•  ,  US  TORE  (NS, NS) 

cot f o n  /rubs/  iol, ins.io. :r  , iss, ib,  irri ,  itF2, itf3 ,  ifdfn.ie, 

<*  IDSTAB.IOLSUG.IStT.rRE;  ,  IPS  b,  I  YU,  I  NORB 

FEAL*#  FBT  (20) 

NS3*NE*NS 

C 

CM  9**  out  PUT  OPTIONS 

C - ICL-1  IF  THE  OPEN  LOOP  ZICENSYSTEB  IS  D  E  S I  RED  — OTH  EFN ISE  I0L-0 

C---I0«l  IF  THE  RBS  VALUES  OF  THE  CONTROL  AND  STATE  ARE  TO  6E  FOUND 

C - TNO-1  IF  ONLY  B  AMD  R  ARE  DIAGONAL 

C  INU-0  IF  A,  B,  Q,  AND  A  APE  DIAGONAL 

C - 1P-0  IF  OrTIBAL  FILTER  AMD  REGULATOR  El  GEN  ST  STEMS  ARE  TO  3£  FOUND 

C  IP-1  IF  EXTERNAL  C  MATRIX  IS  SUrPLIED 

C  IR-2  IF  EXTERNAL  E  IS  SUPPLIED 

C  IP-3  IF  EXTERNAL  C  AMD  *  A  r  F  SUPPLIED 

C---ISS-1  IF  STEADY  STATE  VALUER  ARE  TO  BE  DETERMINED 

C  M«1  IF  MODAL  STATES  DESIPED 

C-»*n 

C 
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nnnn  n  n  nnn 


FOR  r  AT  i 
FOR  F.  AT 
FOR  P!  A  T  ( 
FOR  FIT 
FOR  FIT  I 
FOR  FAT  i 
FOR  FIT 
FOR  E AT  i 
FORtAT 
FOS  FATi 
FOR  F  A  T 
FOR  FAT 
MH  *  Hi 

a  »  n2 


■  '.'OPEN  LOOP  D1NASICS  KATRIX  .  .  .  .  •  ,//) 

6E12.S) 

10J2X.  1PD10.3)  ,/.2X.  10  (2X.1PD10.  3)1 

• 0*,//,2X, 'THE  CfisrfiOL  DISTRIBUTION  MATRII _ • ,//) 

'  O' ,//,2X, 'THF  CONTROL  WEIGHTING  MATRIX . ■  ,//) 

•  O'  ,//,  21,  'PROCESS  NOISE  DISTRIBUTION  MATRIX - •  , //) 


*  0*  , //.2I,  'SEA  SDR  ERE  NT  FEEDTHROOG 


TRII...'  ,//) 


SUEROUTINE  CHECK  CHECKS  THE  CDNSISTENCT  OF  REQUESTED  OPTIONS 


CALL  CHECK  (EPS.  NC.  NG,  NO) 

IF(ISET.EJ.  1)  GO  TO  9015 
DO  9010  1*1  .NS 

9010  R  EA  B  (5.  7444  f  (BA  (I  .  J)  ,  J*  1,  N  S) 

IF  ( IOSTAB  .  EQ.  0)  GO  TO  931# 

REA  D  (5#  7444)  (D  ESTA  B  ( I)  ,  I- 1  ,  NS) 

9014  CONTINUE 
GO  TO  9016 

9015  CALL  SETOP<BA,G,GAM,NS,NG,  NC) 

9016  CONTINUE 
WRITE  (6,3) 

DO  sodi  I*  1.  NS 

5001  VRITE<6, 60001  (BA  (I  ,J )  .  J»1  .  NS) 

IF f I DSTA  B  .  £*).  0)  GO  TO  3999 
WRITE  (6, 4901 

480  FOR  F  A  T  (  /  , '  DESTABILIZATION  CASE....’./. 

•  ■  THE  FOLLOWING  VALUE5  WILL  BE  ADDED  DOWN  THE  DIAGONAL  TO 

*  •  DESTABILIZE  THE  1 '  ABOVE  RATRIX .  OPTIMAL  GAINS  FOR  THE  •, 

»  'DESTABILIZED  SISTER  ARE  THEN  USED',/, 

*■  '  AS  FIXED  SUBOPTI  HAL  GAINS  WITH  THE  ABOVE  SISTER*. /) 

WRITE  (6,6000)  (DESTAB  (I)  ,1*1, NS) 

3999  CONTINUfc 

=  '»EIGLNSTSTEM  O t  THE  OPEN  LOOP  DYNAMICS 
IF  (IOL.  FQ- 0.  AND.  IO.EQ.0)  GOTO  500 
IF  JlOL.Ej.  0,  AND.NC.NE.51  GO  TO  503 
DO  51  I  *  1,NS 
DO  51  J  *  1.HS 
51  GN(I.J1  *  BAJt.J) 

r  Is “•  4»F 


■  Is *•  «»-«»nrs*  (s »f 

CALL  bALANC  (NS,  NS.GH,  LOW  ,  I  H  IGH,  D1) 

CALL  OHTHES  NS,  NS,  LOS  ,  IH I G  H  ,  G  N,  D21 
CALL  OPTRA  NjNS.NS,  LOW.IH  I  JH  ,GN,  02.  SC) 

CALI  HQ*2(HS,  NS  .LOW  ,1  KIGH.GN.CWR.CWI  -  SC,  I  EPR) 

IF ( 1 E PH  .(IE.  0)  CALL  EREXIT  (NS.GN.IERR) 

CALL  EALFAK  (NS,  NS  .  LOW  .  I H I G  H  ,  D 1 ,  NS  .  50 
'■F“,  ■>«  t*  »6r-nA-*R5»* 

NORHALIZE  AND  PRINT  OPEN  LOOP  E IGENS ISTEH 
IWRITE  ■  1 

CALL  CNOFM  (CWR.CWt.SC,  NS,  IS  RITE  ,  HS3 ,  ODD ,  D1 ,  D2  ,  WNC  RM ,  W  MOP  H I ,  HO  ,  CM  , 
•  NO.  NS) 

IP(  IOL,  £0.  2)  RETURN 

IF  (IQ.EQ.  O.OR.  (NC.NE.O.OR.  IPSTAB.GT.Ol  )  GO  TO  600 
DC  496  1*1  , IIS 

IF  (CWB/IJ  .  LT.0.1  GO  TO  496 
WPITF  (6.495) 

•  95  FORFAT  (///'  FROGRAF  TERMINATING  DUE  TO  UNSTABLE  SISTEM') 

FLTUP  N 

496  CONTINUE 

I F  ( 1 0  L  .  EO.  3)  GO  TO  510 
DO  497  1*1  ,  NS 
DO  497  J*1  ,  NS 

497  W11  II.JI  *SC  (I  ,JI 

CALL  RINf  (N33.W11,NS,DDD,ai,D2) 
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DO  505  J«1,H| 

»  fi*0EH  <I.J)*DESTXB(JJ 
DO  567  1-1,  ;s 
DO  507  U*1jfS^ 

ODD  -  0.D0 

§DD526OOo’l  "Sx  (I.  t)  nWSO?.B  I  (K,  0) 

,^«aoc  /t  11  Mtinn 


0-IHT 


DUD  *  UUU  ▼  - * 

Hjs;s/xtJu-i?;si  * o» 

SEXD  (5,7##X)  (IH0(I,J)  ,J^1.»S).I«1,85> 

tfBITfi  16.  Will 

SbItI  (6,60001 '(HO(I,J^,J»1,*S» 

HMto«Ws”o!iSSs.io .  is.  21 
'S18S-i9-*UcKjfV-v.,.c, 

WRITE  (6, 85151 

I  SriIe  (6.60061  (Dd.JI  .J-1.8CI 

I  CONTINUE 


ir®  (»c°.  eo.  0)  «°  10  1SS2 

SttllSAAll.lLll  «*«  9120 

Si?ivroJ\»  «rBGCtl*J‘  •j*,*85) 

WRITE  (6^60pof*S(SJtjJ|)5J*1«*C' 

cxii.nnODEili;ioRsi,G, bb.hs .hs  ,hc,01 

CONTI  HU Ea 
GOTO  <1379 
DO  «0  20  1-1, IS 
DO  “020  0-1 .IS 

lP!llQH,ft-  i?  GO  TO  9215 

’  111 
1  FEXD(5^«99)  j(Q(I,J>  .0-1, HOI  ,1-1,101 
DO  92 17  1*1 .HO 
So  9217  J-1.H5 
D30  «  0.00  , 

s  oSd9-1ddd’*'q(I,k)*h0<k«,1 

7  *^Ttil,6013) 

8  SfITE(6,6005|  (O(I.J)  ,0-1. HOI 

9  SS  tin 

8  S2(i^8H.5,’^MIOH,J1  ♦  XX(*.Il-HO<N,JI 

. . 

0  CONTINUE 

DO  90 “0  I-1,HC 
DO  9090  J-1,HC 
0  S(I,J|,fl-5 


GOTO  9120 


59 


onr> 


U\J  V  U  O  A  ”  1  #  P  ,» 

606  WRITE  (6.6000)  (GJI.Jl.J  »  1  ,  HQ 
ircia.Mf.il  6o  to  Sso^ 

CALL  SODE(WNORHI,G,Bll,NS,NS  ,NC,  0) 

8501  CONTINUE 

IFIIOL  .  EQ.  1)  GO  TO  8505 
WRITE  (6,6003) 

00  607  1*1.  NC 

607  WRITE  16 , 6000)  ( B  (I  ,  J)  ,J*  1 ,  NC) 

8399  IFUtri  .EQ.  0)  GO  TO  8U0& 

OPEN  LOOP  TRANSFER  FUNCTIONS 
8505  WRITE  (6,9220) 

92  JO  FOR  HAT  (■  0*  , //,21,'QPEN  LOOP  TRANSFEB  FUNCTIONS...') 

ITFX*  1 

CALL  TFiNS.NS.Rsg.BA, A A. NO.  G.BH, NO,  HO , CB , IFDFV, 0 , BB.CC.CP, 

*  WR,  Wl,  CWR  ,CWI  ,SC,  JCF.SES.DI  ,  D2  ,  DOD  ,  EPS,  ITF1 ,  I  TFX) 

8400  IF/IOL  .  NE.  3)  GO  TO  850i 
TFjNC  .  EQ.  0)  RETURN 

GO  TO  625 

8502  CONTINUE 

IFJIR  .EQ.  1  .OS.  IR  .EO.  3)  GO  TO  9130 
aeMii'rtno«f  c?  noI ; 1 5  »  r-^i  »  s  *fi 

^■•CALCULATION  OF  CONTROL  GAI  NS :  PORK  ATtON  OF  CONTROL  HAtllLTOHIAH 
C 

C  •  *  *■»  *»»F  AND  FT  APE  THE  OPEN  LOOP 

C  •  *  DTHANICS  HAT R I X  AND  TRANSPOSE 

C  *  r  -GnFBI<iGHT»  4»6BI  IS  NCXHC  CONTROL  WEIGHTING 

C  9  »  SATRTX 

C  *  »  *»»A  IS  THE  NSXNS  STATE  WEIGHTIN 

CO  »  HATRIX 

CO  -A  -FT  6 

C  »*  •»  •»*IH  IS  THE  NSXNC  CONTROL 

C  DISTRIBUTION  H  ATP  I  X 

C*  yrrwrtf  caiti  n**n**» •••»<>■««•  >>*  *■»'»*. i*** at  •  »ci>gBin.nt  t  *r.cnc:-ora« 

00  24  I  *  1.NC 
DO  24  J  -  1 ,HH 

24  tro  ri  ,j)  •  G  |J ,  l)  / b  ( i,  i ) 

DO  25  X  *  1  ,NH 

DO  25  J  -  1 -HH 
RH(1.J«HH)  *0.00 
DO  25  K  -  1.NC 

25  RH(I,J*HHI  -  PS  (I  .  JtRHI  -  G  (I  ,K)  P  PRO  1  K.  J) 

Cr»*2NX2*  rl  AHI  LTONI  AS  SAT  RI  *»•*%» *e »» *  »P 

C - DIAGONAL  BLOCKS - 511  AIID  H22 

DO  26  I  -  1.SH 
DO  26  J  *  1 ,SH 
RHII.Jl  *  BA  (I ,  J) 

FHjlHlH,J*HH)  -  -  6  A  (J  ,  I) 

C - 1121  3  LOCK 

26  FHJIHIH.J)  "-3H(T*SH,J1 

C - H 12  BLOCK  IS  DEFINED  IN  USE  25  ABOVE 

CF»  •r-»oo»  <  r*e  Ai  r  *»*w*  •*■»»»*  t  i*»  **  »  <w'Ci‘l  sus 

50  CONTINUE 

IP  I IDEPUG  .  EQ.  0)  GO  TO  1050 
WRfTE  16, 60141 

6014  FOP  FAt  (//,  '  EULEP-LAG  RANGE  STSTEH  H  AT  RTX  .  .  .  »  ,  //) 

CALL  «APRWT(S,S,n,9,nR,4,'  19(1*.  1PP13.6))  •) 

1050  CALL  BALANC  5,S,  F",  Low.lfiliH.Dlf 
—  -  .  - -  ■■*r,H,RS.B2 


v.ai.4.  Q,  u.i.  .  l.i  ,  ,  r  uu  » t 

CALL  ONTHES  Is.n.LOW.IHI 


C  DEBOG  DIAGNOSTICS  ON  E-L  EO 
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<*>no  nnon  non 


XFlIDEBUG  .  EQ.  0)  GO  TO  53 
WRITE  (8,9115) 

9115  FORMAT]//*  ET. 

DO  52  1-1,8 

52  WEI  TE  16 , 9 )  1 6)  WR(I1,HI{I) 

9116  yOB.“»T  (IX.  1P2D13.6) 

WEI -  ‘ 

9117  FOE 
CALL 

53  CONTINUE 


GVAL  AMD  El  GEE  EC  OF  2»N  E-L  EQ.  APTE8  HQR2*//) 


i “Ail Si  i?iDi3.'6  . . 

:TE  (6.9117) 
i  BAi  |*0*> 

>L  FAPRNT  (M,M  ,M,9,I,tt,  ■  (9  (IX,  1PD13.  6)  >  *) 


IF 
5«  IF 


IF( IDSTAB  .  EQ.  1)  GO  TO  5* 
IF  (NCB.EQ.O)  WHITE  (6 ,91  191 
(9OB.HE.0l  WHITE  (6 .91  21| 
(HOB. HE.  0)  GO  TO  eO 


21,' EIGENSTSF  EM  OP  OPTIMAL  CLOSED  LOOP  SYSTEM..*.// 

2l.'E--- . - - - 


9119  FOR  M  AT  ( *  0*  ,  //  , 

9121  FOSBATj*  O' ,//.2x;*  EIGEHSISIES  OF  ESTIMATE  EF.SOF  EQUATION 
CALL  RGAINfM,  HS.HC.HOB,  HR  ,  W  I,  X,  GN,  W 1  1 ,  SB, 

1 W21  ,  D 1  ,CWH  ,  CWI,  SC,  MBS  ,  D2) 

c  check  e.gt£c 

IF  1 1  DEBUG  .SQ.  0)  GO  TO  753 
WHITE  (6,912  51 

9125  FOR  BAT  (*  EIGEWVECTOBS  FR03  RGAI N  PRIOR  TO  CNORM*) 

CALL  PAFRHT  (HS,  NS,  NS,  9,  SC,  i  ,  •  (9  ( 1  X,  1 P  01  3 . 6)  )  '  1 
750  CONTINUE 

RESET  FLAG  AND  F  MATRIX  FOR  ITERATIVE  DESTABILIZATION  CASE 

IF ( TDSTA B  .  EQ.  0)  GO  TO  9136 
DO  9135  1-1, *5 

135  BA  ( I  ,  I) 

IK-  1 

136  CONTI  HOE 

CALCULATION  OF  FEEDBACK  GAIN 

••FFFDBACK  GAINS - 0  -  -  (  BIKVE8  SE)  »GT»GN 

--CALCULATE  GT 

DO  901  I  «  1.HC 
DO  801  J  -  1 , NS 
PRO  (I ,  J)  •  0.  DO 
DO  R06  K  «  1 ,  BH 


,// 


bV(I.I)  -  DESTAB(Z) 


_  lm j  nww  iinn 

800  PRO(I.J)  -  PfiOfl.J)  «G(K,I| '*G»;r.J) 

801  FBGClI.J)  «  -PRO  (I ,  J)  /i(t.I) 

IF ( IDS?  AB  -EQ.  1)  GO  TO  9130 

HOBBALISE  AND  PRINT  OPT.  REG.  CLOSED  LOOP  EIGENSTSTEB 

IHRITE  -2 

^CALL^CKOKS^CH  R.CWI,  SC  ,  HS,  I W  RITE  ,  HSQ.D  DD,  D1,  D2,WHORP.,  WNOPBI , FH GC, 

•'•THE  orTlAuB’pftDQACK  CONTROL  GAINS 
“  WRITE  (6.977) 

FORBaT(///,*  ■-•THE  CONTROL  GAINS  II 
DO  966  I  *  1 ,  NC 

WRITE  (6,976)  <F8CC(I,J),J  •  1,NS) 

FOR  BAT  ('  *  .  2X  ,  lP6Dt«.  6,/,2X  ,6&1#.6| 

UTS  r.GOAL  C  MATRIX 


9130  WRI TE  16, 977) 

977  FOR  BAT  (///,  •  '.•THE  CONTROL  GAINS  ARE:  1  , //) 

DO  96  6  I  •  1.HC 

968  Wi - "  "*•  -* 

976 

C  COMPUTE 

C  OPEN  LOOP  0-IMVE  SAVED  IN  UNORHI 
IFIIR  .NE.  1)  GO  TO  985 
C  IN  COMPUTING  MODAL  C  RECOMPUTE  U  OPEN  LOOP 

C  SINCE  WHOP M  USED  TO  STORE  U  AN D  U-IST  FOR  CLOSED  LOOP  SYSTEMS,  AND 
C  WHORHI  USEE  TO  SAVE  U-INY  OPEN  LOOP 


DO  8510  1-1, NS 
•  DO  8510  J-1.NS 
6510  UK0FMII.J1  *  WNOSMI  (I  ,J1 

CALL  MTi;»INSO,WHONH,NS,PDD,  91 ,02) 
CALL  NOi,i(  •Niinrt.FBGC,  AA.  *«,  NC.AS  .  31 
905  CONTINOE 

^••THE  CLOSED  LOOP  3THAHICS  MATRIX 
DO  160  I  ■  1 ,  NS 
DO  160  J  ■  1 ,  N3 
SUM  •  0. DO 


61 


onm 


DO  150  K 

iso  sun-soa 


♦o  (i,  ki  *  recc  (*, j> 


160  ACL  (1,4)  «3A  (I,J)  *SUB 

nrite  j6,  nov 

170  FORBAT  (' O'  , 'THE  CLOSED  LOOP  DIN  6BIC5  MATRIX  IS..',//) 
CALL  RAPBNT  (BH,3e,HH,  5,  ACL,  4,  '  IS  (11,1  PD  13. 6)  )  •) 
IF(IR.NE.t.AND.  IR.NE.  3)  GOTO  idOl 
DO  911*0  I* 1 , NS 
DO  9140  J«1,NS 
9140  GHII,  J)  *ACL(X,J) 

"  tiidMtl  i  ui«  cu«oi»M;«r5j*» 


VO  A  OOP  J.  f  fin-ion  aun*  OV| 

CALL  HOR  2  (NS,  NS  ,  LOS  .1  HI3  H,  3  N.CWB  ,CWI ,  SC  ,  IERR) 

IF(  IESR  .HE.  0|  CALI  EREXIT  (NS, GN, IERR) 

CALL  E ALBAN  (NS,  NS  ,  LON  159  ,  D1,  NS, SCI 
cr»r.&e»*e«*c**c=»«r  AO*  a  i-afroi i *  » 

SORBALILE  AND  PRINT  CLOSED  LODP  SOBOPT.  REG.  BIGXNSISTEB 

I  Ml  I T  E  >3 

CALL  CNORB  (CM  R.CWI .  SC  ,NS  ,Iti  BITE  ,NSQ,DDD,D1  , D2 , WHO RB, WSORNI , FBGC, 
•  A  A,  NC,  NS) 

DO  9300  1*1, NS 
IFrcSRIIl.LT.O.O)  GOTO  9  303 
WRITE  (6, 93  10) 

9310  FORBAT  (///,  *  PROGRAM  TERMINATING  DUE  TO  UNSTABLE  CLOSED  LOOP 


9310  FORBAt£///,  1  PROGRAM  TERMINATING  DOE 
OSISTEB' ) 

RETURN 

9300  CONTIHOE 

ir(IO.NE.I)  GOTO  1801 
DO  9600  1-3, NS 
DO  9400  J-1 , NS 
9400  Nil  (I  Ji  -SC  (I,J) 

CALL  rtNV(NSO,H31,NS,  0D0,D1  , D2) 

1801  NCB-NO 

X  F(  NG  .EG.  9)  RETORN 

625  IfJiSET.EO.  D  GO  TO  6  30 

630  ?oJt]n6e 

IFIIOL  . FQ.  3)  GO  TO  9060 
if { i n n.  n e.  0)  Goto  9070 
DO  90  50  I-1.NG 
DO  9050  J-1.NG 
9050  Ofl  .J1.0.0 

906  9  BNli^S,6!r0U,  ««Q(r.J.,3-1.NG,,I-1.N5| 
DO  6  2b  f  .  1,  NS 

626  NRITE  (6,6000)  (GAN  (I,  J)  .  J*  1  ,NG) 

IFCB-nI.I)  GQ  TO  SSSl 
CALL  SOPE(NNOPSI,GAS,  AA,  NS,  NS,  NG,  1) 
8503  CONTINUE 

IP  (TOL  .  ED.  3)  RETURN 
WRITE  16,  60  11) 

DO  62  7  1  -  1,NG 

627  WRITE  16,6000)  (0(1, J)  ,J«1,NC) 

628  If(  (id.FO.O).ARO.  (NG.  ti.of  |  GOTO  389 
DO  ’78  I  -  7,  KG 

DO  378  J  *  1 ,  NS 
*  0.  DO 
DO  J76  k  ■  1 ,  NG 

378  PRDj^  •  PSOJI.J)  *g  (I,F)  »GAB(J,M 


DO  179  J  *  1,  NS 
COd.J)  «  0.  DO 
DO  379  K  »  1,  NG 


379  COII.J)  *  C0(I,J)-GAB  (T,F.)  • 
9100  Iff  1*  EG  .EG.  1)  GO  TO  BdOS 
’■••CALCB  LA  T  ION  5*  FILTER  GAINSiF 


•  FRO(K,  J| 

FOPBATIO*  df  estieatid*  HA-ILTONIAN 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


•• 

-On«Q«CBX* 


* 

a 

n  -HOT*BIS',HO 

•• 


-ft 


* 

•  * 


*»*r  and  rr  are  sase  as  fob 

COATBOL  HAP.ILTOKI  AN 
•  *»Q  IS  NGXNG  STATE  DISTURBANCE 
SCO* ABIAHCS 

>»*a  IS  NOXNO  3  E  ASU  RESENT  NOISE 
RCOXARIANCE 

F*»80  IS  MOTHS  3EASURXMENT  H ATRI 
*»*3S  IS  USING  STATE  DISTURBANCE 
DISTRIBUTION  SATBIX 


C*  t  Ae*cp»<}~  jccc»*=nr 

1805  DO  91  10  1=1, 30 
DO  91  10  J»1,SO 
9110  8C(I,J)=0.0 

- — .  (BC(E.H  .1=1,110) 


BEAD  <5,79441  (f 
WRITE  <6,6052) 

DO  1807  I  =  1,110 

1607  WRITE  (6,6000)  (RC  (I.JJ  ,J-1,  .D 
8506  IF<ITF2  .EQ.  0)  GO  TO  28 

NOISE  TRANSFER  FUNCTIONS 


C 

c 

c 


WRITE  (6, 9230) 

9210  FOABAT(,0*,//,2X,'N 
4  , ’THROUGH  THE  C 

ITF  1-2 
HERO  -0 

CALL  TF  (NS.NS.NSO,  ACL  ,  AA  .  NG  .GAH  ,  BH,  N3,  HO,  CH ,  HERO,  D,  E 
*  WR.fll.CWR.CNIiSC,  JCF.RES.DI  , D2. DD& , EPS . 1TF2,  ItFX) 

IFJIPEG  .  EQ-  1)  RETURN 


NOISE  TRANSFER  FUNCTIONS  • 

LOSED  LOOP  SISTER.  .  •) 

ITF  1-2 

HERO  -0 

CALL  TF  (NS.NS.NSO,  ACL , AA.NS  .GAH,  BN,  NO.  HO.CR ,  HERO,  D,  6B ,  CC  ,  CP, 


28  CONTINUE 

IF  1 1R  EG  .EQ.  1) 
.LT.2)  GOT 
“ , 7441ft )  <( 


GO  TO  388 
0  1811 

FBGE  (I,J)  ,J=  1 ,  NO)  ,1-1,  NS) 


IFflREC 
IPfIR.L 
READ  <5. .  =  . 

GOTO  9320 
1811  CONTINUE 

C*»*  THE  HEASUREHENT  HATBIX 
C*  <  MOTOR  I  N^HO— =  »  >SC 
DO  30  1  •  1 , NO 
DO  30  J  »  1,(1H 

30  PRO  (I  , J)  =  HO(I,J)/RC  (I, I) 

DO  31  I  «  1,HH 
DO  31  J  »  1  ,HH 
RH<  3  4  HH  ,  J)  «  0.  DO 
DO  31  K  -  1  , NO 

J1  P.H(I4HP,J)  »  RH  (1 4  R  H ,  J)  -  HQ(K.  I)  »PR0  <K,  J) 
CT''»G.Y,C'GflT-”>CC 


Ofl 

DO  35 'I  »  1 ,MS 


RBI 
35  R3( 


DO  35  J  »  1 .NS 
RHII.Jl-BA  jt.J) 

'3<i4rH,  j»i!h)  =-ba  <j  ,i) 

GO  TO  SO 

CiiPGC  HACK  TC  50  TO  SET  UP  THE  FILTER  KAHILTONIAN 
C  AND  CALCULATE  THE  FILTER  GAINS 
60  CALL  RGAI  N(3,  SS.NC.NOB,  HI, NX,  A,  GN  ,G3  ,PB, 

1821. 01. CR, Cl, PRO  ,HtiS,D2) 

C  CHECH  ElGT&C 

IP(  I  DEBUG  .  2Q.  0)  GO  TO  58 
WRI TE  (6 , 91  2  5) 

CALL  RAPPNT  (NS.  NS,  NS,  9 ,  P RO,  « ,  1  <  9 <  IX  .  1  ?D  1  3.  6) )  • ) 

58  CONTINUE 

IF(  IDSTAB  .SQ.  1)  GO  TO  9311 

C  SORB  ALI ZE  AND  PRINT  OPT.  ESTIHA  TOR  5IGER3TSTEH 
C 

iwa  ite*» 

CALL  CNOPH  <CR,CI.?RO,  NS.INR  IT  E,  NSQ,  DO  D,  D 1 ,  D2, 8  NOR  H ,  ■  NOP  HI  ,  HO,  AA, 
“  NC.SS) 

9311  DO  61  I-  1..NH 
DO  6  1  J  •  1,30 

61  PRO(I.J)  •  4HO{J,I)  /RC<J,J) 


63 


uou  o  u  uuu 


00  62  I  * 
DO  6  2  J  * 


1,SH 
1  .NO 
-6.  DO 
i  ,afl 


62  ^§"^5 5* tpBO <K* J» 

WRITE  (6. 1501) 

CALL  KAPRNT  (*l  H,  SH.  HB,  5,39,4  .'  (5  <11,  IP  D1  3.6)  )  •) 
WRITE  16, 15  10) 

--  ~  ~  12  I*  1 ,5) 


TATE  GAIKS . •  ,//) 


DO  9  312  T«  1  ,hH 
9312  Iff, I)  *  DSORT  (G*  (1,1)1 
.  WRITE  (6 , 15  20)  (X  (I  ,  I)  ,1*  1 , 5  H) 

9320  WRITS  16. 10  1S| 

1018  FOaMATl'O*  , 'FILTER  STEADY  S  TA 
DO  63  I  «  l.BH 

6  3  WRITE  (6  .  10  19)  (F8GE(I,J),J  -1.SO) 

1019  FOR  fl  AT  { *  ■  ,2X,1P6D14.6) 

C  CONFUTE  SOC1L  A  NATRIX 

C  OPEN  LOOP  0*1)1  S1VED  I*  KNOfiBI 
IF(IB  .RE.  1)  GO  TO  9  330 
CALL  BGDE(WN08BI,FBGE,AA,3H,SH,  NO, 4) 

9330  CONTINUE 

RESET  FLAG  AKD  F  5ATSII  rOR  ITS  RATI ?E  DESTABILIZATION  CASE 

IF  ( IDSTAB  .EQ.  0)  GO  TO  9338 
DO  9335  1-1, NS 
DO  9336  J*1,NS 

9335  BA(I.J)  *  BA  (I,  J)  -DSTORE  (I,  J) 

9338  CONTINUE 

DO  93UO  I«1,N5 
DO  9340  J-1,NS 
SUB *0.0 

DO  9350  A-1.NO 

9350  SUB«SUN*FBGE(I,A)  -HO  ( A,  J) 

9340  - *  "•  "  ‘  - 


936 


)S£D  LOOP  FILTER  DYRABICS  BATRIX  IS..' 


u  JUfl.Jvn.iDutii.iv  -ru 

0  PRO  <I,J) -BA  (I.J)  -SUB 
WRITS  <6.93611 

1  FORBA-f  ('0*  , 'THE  CLOSE. _  _  ... _  _ 

CALL  EAPRRt  INS.  MS,  NS.  5.PR3.  4,'(5(1I,1PD1J.6))  ■) 
irilR  .LT.  il  GO  TO  9  500 
n  — nf-j  uu-  n*  n»ncA— r  ---nn*r  *«o*ftannn  —  -9r 
CALL  3ALANC  (NS.  NS  .  PRO  .  LO  W  I  HIGH  .  Dl) 

CALL  ORTKES  (NS.  NS.  LOW  ,  THIGH  ,  PRO,  02) 

CALL  ORTPAN  (NS,  MS.  LOW.IHIIH  .PS3.D2.GB1 
CALL  HQ  K  2 1  N  s,  NS.LOW.IHIGH.PRO.CR.CI.Gn, 


.  I  ’J,  n  J  ,  .w"  .  .  niuji.  r  pu  ,.n,V4,.»n,  4 

.NE.  0)  CALL  EREXtr  (NS,  PftO.I  ERR) 
BA  N  (NS,  NS, LOW, IRISH, 31.  RS, SB) 

rm  ii>oS»sA-n*  *ro0'4»i»> 


I  ERR) 


.  »2(RS 

IP  ( I S  RB  .NE. 

CALL  EALBA 
r  liKfOiiianroir) 

RNITE  (6.9121) 

RORBAIIZE  AND  PUNT  SUBOPT-  ESTIBAIOP  ZIS-NSYSTEB 
IWRITE-5 

CALL  CKO  SB  <CR,CI,GH,NS,IWRITE,NSQ,DD0,D1,D2.  WNORR  ,  WSORSI 
*  HO.NS1 

no  9ui6  i«i, ns 

IFICH  (I)  .LT.O.O)  GOTO  9410 
WRITS  ?b.  94  20) 

9420  FORKAT  (////,  PAOGSAB  TERBI  NATING  DOE  TO  UNSTABLE  FI  LT  El 
PETURN 

94  10  CONTINUE 

GO  TO  9501 

Q500  IF(IC.EQ.O)  GO  TO  389 
9531  DO  65  I  -  1.RO 
X)  65  J  »  l.BH 
PRO  (I  ,  J)  -  6.  DO 
DO  6b  A  »  1  ,NO 

65  FRO  (I.J)  ■  PRO(I,  J)  ♦RC(I.A)  ■*FB3E(J,A| 

DO  66  I  •  1.BH 


DO  6  6  J  -  1 ,  S  H 
CO  (I.J)  *  O.DO 


.//) 

, HO, A  A , 

') 


64 


DO  66  K  •  1.110 

66  CQJI.JI  »  C5(I,J)  -rBGI(I,M  TRO  (K.J) 
338  CONTINUE 

Ci*ST8E  RRS  STATE  1  HD  CQNToOL  SESPONSES 
IB-IR  +  1 

GOTO  (3360,3360,9380.  9380),  IH 
93  80  DO  97f"  " 

DO  97  C  _  _ 

X  (t  ,J)»0.0 
DO  9705  K* 1 ,NG 


(9360,9360 
05  I» * , NS 
05  J«1,NC 
1-0.0 

uw  7/05  K* 1 ,NG 

9705  D<T:§T^5X^lHsGAn<I'K>*i3<*'  J1 


DC  7710  J-I.NS 
SU3 «0.0 
DO  9711  K-1.RG 
9711  SlM*S0B-1jI  ,R- 


9710 


SUS»S0n-t(X  ,  R ) ft  G  AR I J,  K) 

PSD  (I.J)  »SUS*C0  <I,J) 

pro  !j,I)  -paod.J) 
co ( i,  ji  -sun 

CQJJ.I)  -SUN 

r2t  ii.ji  -3  9(1,31 
«2i  }j,i  -GaiJ.ii 
CAL  L  RIN  T  I  N  SO  ,-2 1 ,  N  S ,  ODD  ,D1  ,02) 

CALL  SCOT  INS, SB, 221, CR, Cl,  NS  ,  3  R.  N  2  1 ,  CR  .  C I  ,  PPO  ,  3  N) 

WHITE  (6,  1501) 

1501  FOR  OAT  ( *  0*  ,//,2X,'THE  COVARIANCE  OF  THE  ESTIMATION  ERROR' 
CALL  HAPR»T{Hn,llH,RH,  5,  GN,  »  .  •  (5  (  1 1.  IP  D1  3. 6)  )  • ) 

WHITE  (6,  15  10) 

1510  rORRATl'O*  ,/,2I.  '  8HS  VALUES  OF  THE  ESTIMATION  SEP 
DO  9715  I*  i  ,SH 


ESROR*  ,/) 

_ ,  .  J5  I*  1  - HH 

9715  Uhl' 

1520  FOR  RA _ 

DO  473o  1«1,NC 
DO  9720  J-1.HS 
SUM -0.0 

DO  9725  K-l.HS 

972S  SUB-SUR-FBGC  (I.  K)  »GS(K,  J) 

9720  X(I,J)-SUR 

DO  9T20  1-1. NS 
DO  9730  J»1,*S 
SUH-O.O 

IF  INC.  20- Q)  SO  TO  97  30 
DO  97  35  K» 1 , NC 
97  35  SUS*S0M*G(I.E|,,*(K,JI 

9730  ^  J2  L1  'jis'j^SI  I.CHP.C*  I.NS  ,CB,R21  ,CR,CI,P?0, 3A) 

IF  (1C.  SO-  0)  ..O  TO  9756 
DO  97  55  1-1,  SC 
DO  9755  J-I.NS 
¥21(1  .J)  -0.0 
DO  9755  E«1  , »S 

9755  V21  II  ,J)  *221(1. JI  -FB3C  (I  ,S)  >'3A(J,R1 
975 ii  DO  9760  1-1. MS 
DO  9760  J-1.NS 
5UR-0.0 


IF  INC.EQ.O)  GO  TO  9760 
DC  97  61  S-1 ,NC 
SU9*SUM»C(I  ,K)PR21  (K,J) 

TR3  II  ,J1  -Slif! 

DO  9762  1-1, IS 
DO  9762  J-I,NS 

PCD  (I  ,J)  -PRO  II.  J)  -CCd.JI  »RRO(J,  I) 

PRO  (J  ■PROfl,  J) 

CALL  SCOT  INs.jC.WI  I.CWR.C*  I, NS, SC, Nil  , 
DO  9770  I-t,SS 
DO  9770  J-I.NS 

3TO  5780 


CNE.CRI.PSO.Cg) 


Gr.  ( 

9770  guJ 
GOT  _ 

9360  CALL  SCOT  (NS ,S C, R 1  1.  CNR  ,C*  I.  NS  ,  SC,  2  1 1  ,  C*i  , C»U  ,3Q,  GR) 


./FI 


65 


UUU  UUl» 


9780  IF  JSC.  EQ.  0)  GO  TO  20  2 


DO  ISO  I  *  '1,  MS 
DO  190  J  «  1,MC 


0.  DO 
1.BS 


DO  TS1  K  *  1,NS 

1S1  ?RO(I,jj*  ?SO(I,  J)  *Gfl(I.R|  CFoGCfJ.M 
190  CCN  TI  NOS 


1.NC 
*  1.NC 

o.&o 

1 1 NS 


DO  200  T 
DO  200  J 
SC(I.J)  * 

DO  231  K  - 

201  sc/r.j)  *SC(I,J)  *F3GC  (I,R|  >  PRO(K,J) 

200  CONCINtJE 

202  IF(  IB  EG  .£0.  01  GO  TO  9791 
DO  9792  1*1,  !*S 

DO  9792  J*1 ,95 

9792  S8J5*filGn 

9791  MRI  TE  16  c  220) 

220  FOHBATl'O' ,//,2l,*THE  COVARIANCE  OF  T9*  ESTI-ATE.  .  *  , //) 
CALL  RiPRNTJSH.RH.flH,  S,Gfl,»,'  (5(11,  1PD13.6))  •) 

IF ( I? -GT.2)  GOTO  503 
DO  67  I  -  1  ,BH 
DO  67  J  »  1,1)8 

;Si  si&a.* 

WRITE  (6.210) 

210  FOB  SAT  ( '  0*  ,//,  21,  *  THE  ST1TE  COVARIANCE  SATRIX. . 
.  ■  " r  taij,5.C0,i  ,*  (5  (IX.  1PD13.  <>))•) 


CALL  RAPRN 


.//) 


IF  (NC.EO.  0)  GO  TO  i 
WRITE  (6. 221) 

FOB  DAT  ('0*  ,//,lX,*T8E  COKTROL  COVARIANCE*,//) 

DO  23  0  I  *  1,NC 

WRI TE  (6 , 23  1 )  (SC  (I,J)  ,J*1,  NC) 

FOR  B  AT  (  1P6  D 16 .6) 

DO  29  0  I  *  1.NS 

CQ  (I.  I)  «  DS0«T(CO(I.  1)1 

I?  (he.  SO.  0)  GO  TO  24 1 


DO  250  I  »1,NC 

250  SC  ( 1,1)  *  D5Q8T<SC(I,I)) 

251  WRI  Tfc  (6 1  262) 

26*  FORBATt'u*  ,  IX.'  STATE  BBS  BE  SPOUSE*  ,23  A,  'CONTROL  RBS  RESPONSE*, 
Yo*  270  1*1  _NS 

.  If  (t-LE-NCl  WRITS  <6  ,2721  CQ  (1,1)  ,  SC  (I ,  I) 

272  FOR B ATI*  *  ,  lPD15.7,25i,0l5.  7) 

IF  JI.iiT.NC)  WRITE  (b,272|  CQ(I,I) 

270  COM  .  I  NUS 

389  IF  ( ITE3  -FQ.  0)  GO  TO  ««0 

FORD  CORTENSATOR  FROH  BE  AS  TD  INPUT  AND  COMPOTE  TF 

DO  610  1*1,95 
DO  «10  0*1  ,  MS 
SUB-0. DO 
DO  W  05  r.-1,!10 

i*05  SUB*SUn»FBGE(I.  Ml  *HO(K,J) 

•  10  COjItO)  -ACL^I.O) -SUB 


3 (I, 2. 

WRITE  (6, 92«(J) 

92*0  FOR  BAT  ( *  0*  ,//,  2  E,  *CCBPSMSArOB  TRANSFER  FUNCTIONS _ •) 

IIP  1*3 
rZEPO-O 

CALL  Tr  (MS.  N5,MSO,CQ,  AA.NO,  PSGE  ,  B*.  ,NC  ,  ?  9GC  ,  CF,  l  E  E  RO.  D ,  SB,  CC,  C  ?, 
•  WF.il,twp.CwJ,s5;jck,B6s,C1fD2,DD6,E?S,ITh,ITFi) 

u«0  CONTINUE 

COBFUTS  PSD  FUNCTIONS  Or  THE  CONTROLLED  STSIEB 

IP ( IPSD  .Eg.  01  GO  TO  »50 
IFjltO  .  LT.  3)  GO  TO  0«* 

CALL  PSDCALIB.MS.RB.X.NC.GM  .CT.FSGC.SO.  H?  «U,HO,  FEGE,  JG, 

1  GAB,  ACL,5A,W?,MI.  Dl,  62.  JCr  .3ES,C,SC.  3B.CC.1  ,I?SD,lsOFB) 
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nnono 


CAL  L  PSDCAL01,NS,an,I,RC,G»  ,GS,  F3SC.RO,  HT.HT.  SO.FBGE,  US, 

1  GA  n.ACL.BA.aR.RI,  01,  D2.  JCF  ,REa,  Q.BC,  3B,CC,2  tlPSD,In08aj 
GO  TO  V SO 

*4*  CALL  PSDCAL(B,l>5,RH.X.!lC,Gi.GV,FBGC,!10,KT,aD,“0,FBGB,RG, 

1  GAB,  ACL, BA, 9 9,  al.ol,  D2.JCF  ,9ES,Q,  3C,  9B,CC,IT7,fpSD,  I  HOBS) 
» 50  IF|ISS  -ta.  3)  SETORS 


IFi  NC 


GO  TO  395 


-  -WE.  0) 

DO  390  1*1,  US 
DO  390  J*1,HS 

11?  ■  3MI-J) 

Clti  KISY(NSQ,  ACL,  MS,  000,01  .02) 
SE1D(S.7U4u)  (99  /  IJ  ,1=1,  WG) 
WRTT#  lh.477  V 


V-  a.  r.l  n  ¥  I  n  J  ,  A1.L.  '  -  ,  uuu,  .  J ^ ) 

SEAD(S.7U44)  (99  (l{  ,1=1,  WC) 

WRIT*  (6,9771)  ( 9  R  (I)  ,  I*  1  ,  M  3  ) 

9771  E09  HAT  (‘0*  ,  •  STE13T  D  ISXUR  3  ANCE  VECTOR . •  ,  /,  10(11,1  PD1 6.  6  /I  ) 

9765  FORnAXt//////.1  STEADY  STATE  VALUES  OF  STATE  VAR.  ARE . ■) 

WRITE  (6,97651 


:te  j 

DO  97  6 
RI(I>  *0 
00  9763 


1=1  ,U 
.  0 

J-1.HG 


HTROL  IS  . ',///, 1 0 ( 1 P  0 1 5 . 5/1  ) 


9763  WI  ( XI  *91  il)  *GAM  (I,  J)A  WB(J) 

DO  976*  1*1, MS 
CP(I)*0.0 
DO  9768  J*  1  ,HS 

9768  CPIT1  *CR  (II -ACL  (I,  J)*  RI  (J| 

9764  WRITE  (b,6060)  C  B  (I| 

DO  9766  1*1,  MC 
CI(I1  *0.0 
00  9766  J* 1 , M S 

9766  CIIII*CI  (Il*FBGCfI,J)<»CR(Jt 
MRI TE  (6 , 97 67)  (Cl  (I )  .  I*  1  ,MC  1 

9767  FOB  HAT  (///,•  STEAD*  STATE  CO 
FETURR 
EMD 

SUBROUTINE  CDIV  (A  , B ,  C ,0, t ,  P) 

I8PLICIT  REALMS  (A-H,  O-Z) 

THIS  SUBROUTINE  C0HPU7ES  THE  COHPLEX  DIVISION 

£  ♦  F»I  *  (A  *  8»I|/(C  »  D*I) 

T«CnC*DC  D 
E«(A3C*E^0»  n 

r*lD»c-A''Di  n 
z 

BETUB  H 
END 

SUBROUTINE  F  APR  NT  (*  BA  X,  (I .  S  ,  l.  A,  IDIM.FRT) 

FEA  L*  P  A  (N  H  AI  ,  Ml 
DIB  EM  SION  FBTllblHI 
NO*  L 

DO  20  NL  =  1,N,L 
IF  (NO.GT-N)  *U»i 
DO  10  1*1, n 

10  WHITE  (6,FN7)  (A(I,J|  ,J=NL.NU) 

WRITE  (6,100) 

100  FOB  BAT ( ’  ') 

20  NU»NU*L 
RETURN 
END 

SOB  POETISE  R7  AIR  (E.ME  ,  WC  ,  NO  B  ,  MS  ,  HI,*  F,  GH,9  1 1  ,  CCS  , 
,  C  7  ,  3HS,  K*“ ' 


1921,LT,C,CI,CT,.1HS,  P.T) 

idflicIt  rsAl*«  «a-f,  o-e) 

DIB  EH  SI  ON  9  9  (■)  ,  il  (RJ  ,VF  (E,  fl)  ,  GN  (MS  ,  9  SI 

DIB  IS  si  OR  W1 1  (NS-rfS)  ,TCS(*  .S}  ,9*1  (NS.NS)  ,1.7  (MSI  ,MT(NS) 
DIMENSION  C(NS)  ,CI  (MS)  ,CT(mS,VS) 


It  •  1 
KP  »  1 
KB  »  1 
BBIEV  »  0 
RCPZE*  -  0 

10  IP(B.  GT.B1  GO  TO  200 
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nnfjo 


CHECK  ?08  E1GVAL  AT  0*  NEAR  J-3HEG1  Alls  TO  IHCLQDE  IK 
TDRS  f  IS  ST  ONE  POSITIVE  AND  SECOND  CNE  NEGATIVE 


E-L  EIGSTS 


El 

T  r 


13  VS-DAES  (KB  (K)  1 

riEIGVR  .  GE.  l.b-10)  GO  TO  48 


iriEIGVR  . 

IPINRIEV  . 

NF{K>  *  El 
GO  TO  75 

HK(K)  *  -* _ 

KRTTE  (6.9000) 

9000  ros/tAT/’o*  ,■  EULER-LAGRAN3E 
*  •  Ob  NEiB  ZERO.*/) 


IFlBl(K))  40,00,40 
20  NREEV  «  NPZEV»1 

1 P ( N  R  ZEV  .CT.  1)  GO  TO  35 
KR(  K)  «  EIGVB 
GO  TO  75 

35  UK { K)  *  -EIGVR 
RITE  (6  , 


EQUATIONS  HAVE  A  REAL  EIGENVALUE  AT', 


GO  TO  110 

40  NCPZEV  *  NCPZEV«-1 

IFINCPZEV  .GT.  1)  GO  TO  »5 
KR(K)  »  EIGVR 
H  R  i  K  ♦  1)  «  EIGVB 
GO  TO  BO 

45  NP.(K)  *- EIGVB 
KRjK*11  *  -EIGVB 
16,9010) 

EULES-LAGRAN3E  EQUATIONS  HAVE 


KFITE  (6,9010) 

9010  FOR  HAT  ( *  0*  ,  '  EU  LES-LA  GFA  NGE  EQU  A 
•  *  SIGENV  A  LUES  AT  OP  NEAR  THE  J 

GO  TO  120 

48  IF(KRIK))  1  Ou  ,50,5 
50  If(KI  kU  00,75,&0 
C  - EIGENVECTOR 


A  COMPLEX  PAIR  OF 
ONE! A  A1IS.1) 


_  TOR  REAL  EIGENVALUE,  POSITIVE 

75  IF ( NO B,  EQ.  0)  GO  TO  78 
00  76  J  ’1,1 

76  TCB  (J.KP1  *  VF(J.K) 

78  KF  *  KP* i 

K=R  •  1 
GO  TO  10 

- EIGENVECTOR  FOR  COMPLEX  El  GENV  ALOE ,  PCS  ITI V  E  FEAL  PART 

80  IF(  NOB  .  EQ.  0)  GO  TO  83 
DO  81  J  -  1,11 

PR  »  VPJJ.Kf 

ri  -  -vr<J,k*ii 

TCB(J.KP)  *  FR*PI 

81  TCB  (J,r,P*1)  -  PR-PI 

83  KP  *  Np»  2 

K  »  X  *2 
GO  TO  10 

100  IF(Kl(RH  120,110,120 

- VlGENVECTOl  POR  REAL  EIG  ZN  V  ALL’  E ,  NEC  ATI  VE  REAL  PART 

110  C  IK  P]  *  VRIK) 

CI(KN)  «  Kl  <K» 

IP  (NOE *  NE .  0)  GO  TO  96 
K  NS  -  KN*NS 
DO  95  J-  I  ,H 

95  TCB  (J,KNS)  *  VT  (J,K) 

96  KN  •  K»«1 
RrK  ♦  1 

GO  TO  10 

- EIGENVECTOR  FOR  COMPLEX  El  GENV  ALUE,  N  EG  ATI  V  E  REAL  PART 

120  RR  ’  NR(K) 

1!  *  WI  (K) 

I(KK)  *  F.R 


PI 

C  (KK) 

C  AN*1|  •  A  a 
CI(KN)  *  RI 
Cl (  P  N  *  1  )  -  -  RI 

IP  (NOB  -  RE .  0)  SO  TO  122 
F.  NS  ■  r  N*NS 
DO  121  J  -  1,(1 


pp  ■  vrjj.Ki 
p:  *  -vfi(3,k*ii 

TC3  (J  ,  K  NS)  *  FB 
TCB  (J  ,KNS»  1)  * 


121  .  >-U  |W  ,  n  p  _ 

122  KS  ■  k**2 


•ri 

rs-ri 
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non  non 


K  •  K  *2 
SO  TO  10 
200  CONTI  NOE 

IF  (NOP.  HE.  0)  GO  TO  321 
C  POFHATION  OF  Nil 

DO  356  i  *  i,  ns 
DO  300  J  »  1 ,  NS 
N1MI.J)  *  TCBfI.J 
300  CT(I.  J1  -  Mil  (I,  3) 

<-  FORMATION  OF  V21 
DO  320  1*1,  NS 
DO  320  J-1.NS 
M21(I,J)*  TCB  <I*NS,  J*  NS1 
IT  (NOB.  EO.  0)  GO  TO  323 
DO  32  2  I  *  1,NS 
DO  322  J  « 

M21 
Mil 
CON 

INVERT  Mil 
NSD-NS'Hf 
ILL  - 


320 

321 


32  j 
32 : 


J!  I  yj  “ 

I  (I ,JI  - 

Wifiol  * 


.  J*NS) 


1,  NS 
1.  NS 
-TCB  (I ,  J) 
TCB(r«flS,JJ 


CALL  HINWNSQ.MII.NS,  DETC.LT.M) 
CAICULATE  THE  RSilN  MATRIX 
DO  325  IL*1,NS 
DO  325  JL-1.NS 
GN(IL.JL)  *  0  .  DO 
DO  325  «t«1,NS 


125 


DO  5*99  I  •  T,  NS 
DO  5999  J  »  1.KS 

5,99  SHii?  ’  "11{J'I» 

END 

SUBROUTINE  NINV  (NSQ.A.N.  D,  La  N) 
IMPLICIT  REAL»8  (A-fi,0-2l 
DINFNSION  AjNSOI  ,  L  (tl)  .11  (N) 

DOU ELS  PRECISION  A,  D,  BIG  A,  HOLD 
NH»N**N 

0*1.006 

N  K  *  -  N 

DO  60  K*1.N 
NK-KK.R 
L(K)  -K 

n  Ik)  *k 
KRaNK*K 
MG  A*  A  (KK| 

DO  20  J-K,  N 
IZ*  F°  (J-l| 

DO  20  I*K, R 
IJ*1Z*I 

10  IPJ  OAHSfDIGA}-  DAES(A(IJ|)>  15,20,23 

1  LINjii*1 

NjKi *J 
20  CCNTI NOE 

INTERCHANGE  ROMS 


’(J-ll)  35,35.25 


J 

IF( 

25  RI-F-N 

DO  30  I-1#» 
KI-HX+N 
HOLC--A  (ICI) 
JI-FI-MJ 
A  (KI)  *k  (.II I 
30  aJjI|  -HOLD 

IATEPCHANG • 

35  X-i  (K) 


COLUSA  5 


69 


nnn  non  onn 


38 


40 


U  5 

U  6 

US 

50 

55 


irtl-KI  05,45,38 
3P-X*  tl- _ 

DO  40  3*1.* 

JK*KK*3 

JI*JP*3 

HotD*-x<JK} 

DIVIDE  COLUEM  BT  MISOS  PIVOT  [VXEOE  OF  PIVOT  ELSHE8T  IS 
CO 8T MSEC  I*  8IGX) 

IF  (  dI  GX)  48,46,48 
3*0. f “ 


D*d . 0  DO 
RETURN 
DO  55  1*1.8 
IF(I-K)  50,55,50 

pedoce  hxtbix 

DO  65  1*1,8 
IK*8K+r 
HOLI*X( IM 

DO  65  3*1*8 
I J*  I J  *8 


60 

62 

65  C 


tF f f  =  K|  60,65, 
—  ( J  -  KL  62,65, 


,60 

62 


5jIJL-HOLD««MKJ)*MIJ) 

"ION  TI  80S 

DIVIDE  808  BI  PIVOT 

KG«K-N 
DO  75  J-1.8 

8J*KJ4» 

IriJ-M  70,75,70 
70  X  1KJ)  «XlM)  /b!g» 

75  CdNTTNUt 

PRODUCT  OP  PIVOTS 

D*0r  6IGX 

P.FPIXCE  PIVOT  8T  RF.CIPBOCXl 

X^jjKJ  jjl.OOOl/SIGX 

FINXU  ROM  X8D  COLU  S8  INT  ERCH  XRGE 

R*N 

50,  150,105 

IM^-ltl  t20.120.10B 
30*  N®  ik-1J 
JK-nMI-U  . 

DO  HO  J*1 , 8 
0  M  JO  *0 
HOLD*  X  (0E| 

MOrj.-xjoi, 


BO 

100 

105 

10fi 


110 

120 

125 


X  (3  Tl  *H 
J«R  1*) 

IF(J-K) 
kw.-j  _  .  . 

DO  130  I*1,» 


100,100,125 


70 


K  I*  KI  ♦! 
HOLD*A(KH 
JI“ KI-R+J 
A  (K  1} 

1J0  iblS 

GO  TO  1 00 
150  K*0 

RETUB  I 
END 

SUBROUTINE  SCOT 

REALMS  . 

•  V  R 1 

PEA  L»8 
100  DO  50  - 

DO  50  J  =  1,  NS 


(Kli{  ,VR2(*R1  ti'S,(5R.»*)  i«<'l  (NR.NRI 
IA,&  C.D.K1.*2,lf3.fc4 
1*1, II. 

„  J»1,NB 
X  (I  .J)  »0. 

DO  SO  11*1, «L 


KR| 


50  Jl’  *HLI  fI>I  T>  >0<I  X‘  ^ 

DO  52  J«i'nR 
0  (I ,  J)  -0. 

5 1  •** ♦*  (*•  JJ)  *1*®1  (J.JOI 

13  IF1  (TL2(U  )  ’0,  11,10 

10  «J*1 

14  IF(VF2(JM  20.21,20 

C-A?*  2»VlJ  (1)  **  2*  vt  2  ( JJ  A  *2 
D«C''>2-B  r*  j 
Mi  if  C/D 

K2»-  (VR2  (J)  2c*'ri,S 
KJ*  -  (TR2JJI  r  B*TL2  (X)  •  cj  /D 
K4--A**B/D 
11*1*1 
J  1*J*  1 


(I  1  ,  J')  -»N49( 
*J  ♦  z 


If  J) -K«^Q  I1»J  ♦KS  3(J  J 

hj)-K2*2  I.  Jl|-K2*Qjn.J  ♦K1"J(t1,J’ 


21 


GO  TO  22  ..... 

X  «!jf*N^*Q(I»J)“K2»0  (I*  l.j) 

X  jl  *1  ,  J)  *K2*Q(l'j)  •Kl*'Q<I*1  ,•») 
j»j*1 

22  ir«J.  LE-NR)  50  TO  14 
1*1*2 
GO  TO  12 

15  IF1  [VP2(Jn  30,  31,30 

xjl|j»1)  »R  2  }Q  (I,J)  *Ni|Jg(l,J*1J 
J-J*2 

31  Xjt  jj) -5  (X,  J) /(»?’ (J)  ‘TLian 

32  irJ*J.LF.N8|  50  TO  15 
1*1*1 

12  IF  [t .  IE. Nil  GO  TO  13 
DO  40  1*1, iL 
do  40  j-  i,  ia 
0(1.  Jl  *0. 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


«L(X,XX)»X(IX,J) 


ENSIo/wNORM  (*1  ,H2>  .GMORM  INI  ,  S3) 

HAT(“o‘. //,"*. ?BOdV1  CONTROL  51 STR  f  Blit  1  ON  MATRIX..',// 
0>  .//.2X.  '  MODAL  PROCESS  NOISE  DI S7F IFUTIO H  MATRIX 

•  X.  S'  i  «nr>  ,T  HI  ellBPUFWT  FCll.rWf-  KiTBTTt  / 


DO  40  11*1  .  Nl 

40 

DO  *2  J*1,  NB 

41  X°flij,*X  JJ) 

R 2  CONTINOE 
RETUB* 

15?  BOOTIliE  BODE  ( WRQBM  ,C,M3  , 12,  IC'X| 

WUORM  TB  AHSFOR5AT ION  MATRIX  U  OB  O-IHf 
!g  SS‘  °orSLSP0TS  OR  OUTPUTS 

ICO*  CONTROL  Pile  TO  INDICATE  RHICB  TRANS  POF -ATIO* 
0  •  NODAL  a 

1  .  NODAL  GABfU 

2  •  NODAL  H 
5  *  NODAL  C 

5  l  CONTROL  TICE  NVECTO  R  MATRIX 

6  -  (iEASUF. ENENT  EIGENVECTOR  RATRIX 

IMPLICIT  P  EAL"8  (A-H.O-Z 

DIN  . 

65  00  FOR _ 

6501  FORMAT 
b570  FORHAT 
6580  FORMAT 
6584  FORMAT 
6586  FORMAT 
6000  FORNAT 
6590  FORMAT 
DO  SO  - 

DO  50  J* - 

50  GN0  4H  (I.jj’ “0. 

GO°  TO  (75,751250,250.  75. 250. 250). IPOtNT 
75  DO  188  rAiil 

100  ls3?5^5 ?Sir 5ls* 34 lift oT"237  .(HoUt(K,j’ 

105  * 81  TEjfb, 65001 

120  UNITE  (6,6060l  (GRORN  (I  ,0)  ,J*  1,»2| 

RFTUn  * 

150  RHITE  (6,6501) 

GO  TO  1 10 
160  WRITE  (6,b590) 

GO  TO  1*0 
250  DO  260  J«1  ,  NS 
DO  260  1*1, HI 
DO  26  0  K*1,NS 
26  0  -  *  ’*  - 

26 

262  WRITE  (6.6580) 

GO  TO  270 

263  WRITE  (6.6594) 

GO  TO  270 


1  WRITE  (6, 65*0) 


WHLTK.  10,0 
GO  TO  270 


2#>u  WRITE  (64tS86) 

27?  WFITE  (6)bo6o)  (GNORr.  (I  ,J)  .J*  1.MS) 

RFTUS* 

SUB  ROUTINE  CRORMNZ.NT.VEC.  NS.  I  WRITE,  NSQ.  ODD.  D1.02.NSORB, 
P  HO,  CM,  N  1,  *2) 


VZ(I) 


REAL  PART  OF  I-TR  EIGENVALUE 


WROR  MI, 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


WT(I) 

VEC 

*5 

I  WRITE 
NNORB 
UNO  RBI 


CORPLEX  PERT  OP  I-TH  EIGEXVJkJ.DE 

matrix  or  right  eigenvectors  stored  in  reel  porn 

PROS  HgR2_ 


NO.  OP  STATES 

FLAG  TO  CONTROL  PORHAIS 


POR  DIFFERENT  EIGHPNSTSTERS 


Korn ali zed  hatrii  d  op  riggt  eigenvectors  stored 

ST  COLOURS  IK  SEAL  PORN 
D-INVERSE  2»CONGUGATE  DP  LEPT  EIGENVECTORS 
STORED  BT  R OV  IK  REAL  PORH 
SSQ,DDD,D1,  D2  -  ARCUNENTS  PASSED  TO  NINV 


IBP  LICIT  REALMS  (A-H,0-Z> 

REA  L*8  FIELD,  COMMA  .  siMCOL.R  IG.,T,  PST 
DIMENSION  VZ(NS)  ,  Vt  »NS1  ,  VEC  (NS ,  NS)  ,«  NORM  (NS  ,  NS) 

^  jKNORMI  ^NS,  NS)  ,  STORE  (b|  ,  D1  (  NS)  ,  D2  (N  S)  ,  F NT  (14)  ,  HO  (N  1  ,  H2)  , 

DAI  A  ri  ElS/sIiEI  2.  5/.CONMA/5  H,  /,  S  ENCOL/SH,  • :  • 

1  ,RIGHT/1H>  /,  FNT/6K  (U,  IP  ,U(  1H  /,  5  EH  END/UH  ,  •  :  •  / 

9030  POR  M  AT  (  '  O'  ,  '  OPEN  LOOP  EIGENVALUES..’) 

90«S  POR  N  ATI* O’,  ’CLOSED  LOOP  OPTIMAL  REGULATOR  EIGENVALU 


_  .  _  _  _  _  _ ALOES..’) 

•O',  ‘CLOSED  LOOP  SUR0P7IM.AL  REGULATOR  EIGENVALUES., 
•O'  .'CLOSED  LOOP  OPTIMAL  ESTIMATOR  EIGENVALUES..') 
rw.i„  ,  'O’  ,  'CLOSED  lOOP  FUBOPTIHAL  ESTIMATOR  EIGENVALUES., 
FOR  MAT(//'0', 'RIGHT  EIGENVECTOR  MATRIX.. '//I 
FOR  MAT(//'0' ,  'OPEN  LOOP  LEPT  EIGENVECTOR  MATRIX. .'I 
FORMAT  {//'O',  'CLOSED  LOOP  DPT.  REG.  LEPT  EIGENVECTOR  Ml 


9050  FORMAT 

906  0  FORMAT 

907  0  FORMAT 

9080  - 

9090 

9100  _ 

9110  FORMAT 
9120  FORMAT 
9130  FORMAT 

C  11  fLrMAT  («6I.'  (•  .F10.7,')  «J(*,P10.7.  *1  •) 

C  NORMALIZE  COMPLEX  EIGENVECTORS 
C 


.  ,  _  .  _  _  _  _ _ _ _  ATRIX..  •) 

//•  O' , 'CLOSED  LOOP  SUBOPT.  REG.  LEFT  EIGENVECTOR  MATRIX..' 
//•O', 'CLOSED  LOOP  DPT.  FILTER  LEFT  EIGENVECTOR  MATRIX,  .i') 
//'O', 'CLOSED  LOOP  SUBOPT.  FILTER  LEFT  EIGENVECTOR  MATRIX. 


BT  LARGEST  ELEMENT 


KK*0 
LR»0 
LC-  3 

TO  999  R  -1.NS 
IKCKK.EQ.il  GOTO  991 
IP  (DABS  (VV  (K) )  .LT.  1.  D-10| 

LC*  LC  *  1 
?HA  X  •  0.  DO 
DO  997  I  *  1,HS 

CMOD*vEC(I.K  )  6»2*V  EC  (I.  K*1)V*2 
IF  (C  HOD- EH  AX)  997, 9  90 ,99  0 
990  DMA  X  »  CMQD 
N*I 

997  CONTINOE 
VMR  •  VEC 

(All 


GO  TO  999 


SCIM.K) 
FC(M,A» 
[*1,  NS 


VMI  *  VEC 
DO  980  I* 

TK  *  VEC  (X  ,  K) 

VI  *  VEC  II  ,  K*  If 
VUC5N-(VK»  VMR»VI»  VMI)  /EM AX 
VFE  IN  •  f  -  VR  t‘  VMI*  V  1 9  V  HH  )  /  EM  A  X 
UNO  AM  CI.K)  *T»CRN 


NO  AM  II 
FORM  (I 
DN  TI  NO 


r*  1)  *VECIN 


VKO 
980  CON 
r,r-  i 
GOTO  999 
991  KF-0 
999  CONTINOE 
C 

C  NORMALIZE  REAL  EIGENVECTORS  at  THE  TOTAL  LENGTH 
C 

DO  1000  r*1,NS 

ir(3ABS(VT(R)  )  .GE.1.D-10)  GOTO  1003 
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998  LE»  LB*1 

RG10D  *  0*  DO 
DO  996  1*1.  US 

996  R£HOD*YEC(I.K  )**2*BE80D 
RH3D*DS0ST  (REHOD) 

DO  995  1*1,  NS 
RVEC-YEC  (X,  XI  /R  HOD 
WHO  an  II.  K)  *  BY  EC 
995  CORTIpDE 
1000  COH TIROS 

c 

GO  TO  (520 ,530,  5*0 , 5U  5,550)  .IWRITE 
520  WRI  7E  (o,  9o5Q) 

GO  TO  SiO 
5  30  WRITE  (6,90*0) 

GO  TO  560 
5*0  WRITE  (6,9050) 

GO  TO  560 
5*5  WRITE  (6  ,  90  6  0) 

GO  TO  560 
550  WRITE  (6,9070) 

56  0  KK*0 

NPi7N*0 

NFHTN*1 

DO  568  1*1, RS 

IF  ( RE  •  £0.  1)  GO  TO  567 

If(uASS(WI  (l|)  .GT.  1.D-10I  KK»1 

C  PRINT  OUT  NO  HORE  THIN  6  WORDS.  NOT  SEPARATING  COMPLEX  EIGVAL 

IFMJPhTB  .IT.  5  .  OP .  ( NPRIW  .  EQ.  5  .AND.  XX  .  FQ.  0))  GO  TO  561 

FHT  (N  FHT  W*  1 )  *  RIGHT 

WRITE  (6. rat)  (STORE  (J)  ,J*1,  NPRTW) 

NPRTN»0 

NfHTN-1 

561  NPRTW.srPTWH 
NFHTW  *NFNTW*1 
IFJKX  .  EO.  1)  GO  TO  562 


STOKE  (NPRTW)  *  WZ(I) 
FflT  (A  FHTW)  -  FIELD 
NFHTW  *  NF HTW ♦  1 


562 


567 

568 


570 

575 

578 

580 

590 

600 

610 


FF.T  (NFHTN)  *  SCHOOL 
GO  ?0  568 

STD  PE  (NPRTW)  *  wim 
FHT  (NFHTW)  *  FIELD 
FHT  (NFHTW*  1)  *  COHHX 
STOSl  (Kl  FTW*1)  -  WTJI) 

FHT  (NFHTW*  2)  •  FIELD 
FI»rjSFHTW*3(  -  SEHCOL 
NFHTW  -  NFHTW  ♦  3 
NPRTW  *  NPRTW  ♦  1 
GO  TO  568 
KK*0 

CONTINOS 

FHT  (NTHTW)  •SEHEND 

FHT  (NFHTW*  1)  *  RIGHT 

WRITE  (6. FHT)  (STORE  (J)  ,J*1,  NPHTW) 

WHITE  (6, 9080) 

CALL  RAfRNT  (NS,  »S  ,  »E,  6,  W  NOR  H,  *,  •  (6(11.  1  PD  13. 6))  *) 
CALL  SAPRNT  JNS.NS.NS.o.wNSRH.w,  ■  (6  ( 1?G  1  2.  b|  >  ■) 

GO  TO  (5  78-570,576,  5(5. 5 75 1  .IwfcltE 
CALL  HODI(6wORH,HO,CK,NS,N1  ,N2,  5) 

CO  TO  578 

*ALL  HODEfWNONH.HO.CK.  NS.S1.N2,  6) 

GO  TO  (530. 590, 606.b30.o23f  .IWRtTE 
WPITE  (6.9090) 

GO  TO  630 
WPITE  (6  .9100) 

GO  TO  b30 
WRITE  (6,91  10) 

GO  TO  6  30 
WRITE  (6.9120) 
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« 

•  nn 
SAVE  COHPU 
IFfITPX  .E 
IFjlTFX  .E 
CALL  POLES 


CO  TO  630 
620  SUITE  (6,91301 
C  SAVE  3-INV  O  PE  K  LOOP  IS  WNORMI 
630  IF(IGRI?E  .GT.  1)  GO  TO  1035 
DO  510  1*1, NS 
DC  510  J*1,NS 

510  UKDF.Itlll.Jf  =  b'MOBfl  fl.JJ 

CALL  flrNV/HSQ.SNOani,  NS,  030  (  01,  D2> 

CALL  RAPRN?  (MS,  NS,  MS,  6, SNOB  MI, 4  ,  ‘  (6  (1 X,  1  PD1 3.  61 1  ■) 

RETURN 

1005  CALL  MIN  V  (  SSQ.NNORM  ,NF, ODD, 01,02) 

CALL  FAPRNT  (NS,  NS  ,  NS  ,  6,i  NOR  R,  5.  •  (6  ( 1  X  ,  1  PD  1  3.  6)  )  • ) 

RETURN 
END 

SUBROUTINE  TF  II  ,N  S,  NS  Q,A  ,A  A  ,B,  B  ,  B«,  L,  C,  CM  ,  I FDFH,  D .  BE.CC.CP. 

•  EVR.£VI,PR.  PI,  SC,  JCF.SES,  D1 , 02  ,  ODD  ,  EPS,  IIP,  ITFX) 
ir.?LIClf  SEIlL'8  (A-H,fi-Z> 

DIHFNSION  A  (N  ,  N)  ,  AA  (N  ,Nl  ,3  (  N,  M  ,  BS  (N,  fl)  ,C  (L,M»  ,CM  <L,  HI  ,D(L,H1  . 

•  BB|N).,CC(Ni  ,CP  <  «r  .  EY6  (l»>  ,EVl(N(  .  PR  (H)  .  Pi  (N)  .  SC  (N,fi)  .JCF(Nf  . 

POTAtfoON  Ofc  OL  AND  CL  SIS  WITH  MODAL  HORN  DONE  IK  OPTSTS 
“  1)  GO  TO  50 

3.  2)  GO  TO  5 

t-tt  rum  (N,  NR,  A.  A  A,  H,B,L.  C,  PR  .  PI  ,D  1  ,  D  2  ,  JCr  ,  SC| 

;  COMPUTE  MODAL  MATRICES  FOR  aESIDUES 
$  DO  10  1*1,1 
DO  10  J*1,H 
10  AA(I.J)  *  SC(I,J> 

15  DO  26  1*1, L 
DO  20  J- 1 , N 
CS(I,J]  *  0.  DO 
DO  10  k»1,N 

20  CE(I,  J]  -  CM  (I.  J)  ♦  C  (I.K)  »  AA(K,J) 

CALL  nlNT(NsO.A  A,N.DD0,C1,D2) 

DO  30  1*1, M 
DO  30  J-1.H 
SB(I,  J)  *  6.  DO 
DO  ifi  K«1,N 

50  cC.ihfL'0,la^3l  *  CI.K)  »  B(KrJ> 

DC  100  1*1,  H 
DO  100  J*1,L 
I  F(  ITF  .  NE-  3) 

"CnLL  ZEROS  ( I,  J,  ! 

•  EPS) 

IFflTP  .NE.  2)  CALL  RESIDd  ,J ,  N  ,  JCT.  H,  B  H  ,  L  ,  CM  ,  PR  .  PI ,  R  EE  ,  OB  ,CC  ,  1) 
100  COHII  NOE 
RETURN 
END 

SUBROUTINE  CHECK  (EPS,  NC,  HI,  NO) 

DOUBLE  PRECISION  EPS 

COMMON  /FROG/  IOL  ,  I  HQ  ,  IQ  .  I  h  ,  I  S5  ,  IH,irri,I7r2.ITP3,IFDFW,IE,n'STAB 
«>  I  DEBUG,  IS  ET,  I  REG,  IPS  b.  I  *U  ,  I  NORM 

I  SET  MODAL  ASALlSlS  WHEN  OL  tf I E  SI S  ON  JL  TF  REQUEST  ED 
ITIIH  .  EG.  1  .AND.  IOL  .10.  Ol  I0L»1 
IF(  IOL  -F0-  3  .OR.  ITF1  .RE.  0)  IS*1 
:  CHECK  TO  SEE  IF  H  MATRIX  INi’EH 

IF(  NO  .NE.  0  .OR.  IOL  .It.  2)  GO  TO  25 
WRITE  (6 ,8000) 

9000  FORMAT!//1  H  -  MATRIX  MUST  BE  INPUT,  I.E.  NOB  .ST.  0*) 

STOP 

25  continue 

THANSFEB  FUNCTION  CHECKS 
IE*6 


rIPOrH,  N,N*,A  ,AA,M,B,L,C,D,BB,CC,CP,  EVt!,  EVI.D1,  D2, 


C 

c 

c 


if(ie  .so.  01  : 

EPS  *  1  0. 11  *  (-IE) 

OPEN  LOOT  TP 

IPIIIF1  -TO.  0  -OR.  NC  .NS  .  Ol  GO  T3 
■  E  ITE  (6 , 9300) 

9000  FOR  HA  ,  (//*  IN?UT(G)  MATEIX  MUST  BE  t  SQ0ES7EI1  (l.E.  SC  .  NE. 
»  'ZO  CORTOTS  OPES  LOOP  T .  r.  ') 


0) 
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STOP 

C  COHPENSATO?  IT 

SO  IF ( ITF3  .IQ-  0)  GO  TO  100 
IP  ?  IF  EG  .  EO  •  0  .AND.  (NC  .  H 
WHITE  (6 ,9100) 

9100  FOBNATI//'  8 
9  .•THE 
STOP 
100  CONTIN0E 

c  noise  TF 

~  0)  GO  TO  15  0 

'  ■  -*  .HE.  0)  GO  TO  150 


SEGOLATOS  AND  F 
SANE  RUN  TO  CDSPtJ 


S.  0  .AND.  SG  .  NE.  0()  GO  TO  100 

ILTEB  STNI H ESI S  SOST  BE  SEOOISTED  I*  • 
TE  COHPENS  A  TOB  T.  F.  • ) 


IFIITF2  -EO  _  __ 

IF)  NG.NE.  3  .AMD.  NC  . 

WRITE  (6, 92001 
9200  FOB  RAT  (//'  NOISE  T.  F.  CALC 
0  ,  1  AND  GANNA  INPOT.  I.  S 

STOP 
C 

C  DESTABILIZATION  RESTRICTIONS 
150  IP(  IDSTA  5  -EQ.  0)  GO  TO  200 
IF  ( NC  -EO.  0)  GO  TO  2  00 

if!ng  .  nL  of  ibeg-i 

WRITE  (b  .95  00) 

9300  FQRHAT(//*  DESTABILIZATION 
•  • FILTEfi  BUT  NOT  BOTH  SI 

IFjlHEG  .EO.  1)  GO  TO  200 

200  CONTINUE 
C 

C  PSD  INPOT 
C 

IFCIPSD  .EG.  0)  GO  TO  300 
IFIIPSD  -Lf.  0  .OR.  I PSO  .3 
IF  1 1 1 0  -LT.  0  .08.  IIU  .GT. 
IF(INOPN  .LT.  0  .OB.  1N0S5 
GO  TO  275 
250  WRITS  (b.PuOO) 

9900  FOHflATl*  **l4l**»»  INCONSIS 
STOP 

275  IFIIPEG  .FO.  0  -AND.  NC  .  »E 
WRITE  (9,95001 

9500  FOr-  NAT  (  ’  ***-fc<  ••BOTH  A  R  EGO 
1  'TO  CORPUTE  THE  PSD  OF  A  C 
STOP 

300  CONTI  NOE 
RETURN 
END 

SUBROUTINE  FOLPS  IN,  NB.A,  A*. 
IMPLICIT  REAL"B  (A-H,0-Zj 
DIMENSION  A(N.N)  ,  AA  (H,«  ,B  ( 
<“  JCFfNJ  ,SC  (N.Jl) 

DO  1 
DO  1 

CALL  EALASC  (NN,  N,  AA.LOW,  IHI 
CALL  OFTHESfNB,  N,LC».  IMGH, 
CALL  ORTFAN  INN.  N.LOW,  IHIGH, 
CALL  HOR2I  NN,N,LCV,IHIGH,AX 
I  F  ( I  ERR  -  N  E.  0)  CO  TO  110 
CALL  EALEAIUNH.  N.LOW,  IHIGH, 
Cr  ■•■eel  •• 

WRITE  (6.  1011 
- IT(//V,2l 


OLATED  ONLT  WHEN  REGULATOR  DESIGNED  • 
.  NG  . NE.  O') 


OPTION  DESIGNED  FOB  A  BEGOLATOB  OB  •, 
MULT  AN  SOUS  LI  •) 


T.  3)  GO  TO  250 
21  GO  TO  250 
.GT.  NGFNO)  GO  TO  2S0 


TENT  PSO  INPOT  FLAGS  »»•*•«■*•) 

.  0)  GO  TD  300 

LA TOB  AND  FILTEP  HOST  BE  P  ESIDFNT 
ONTROLLED  SYSTEd!') 


B.B.  L|C,EVB,ETI.D1,02.JCE,  SC) 

N,N>  ,C(L,M  ,EVR  (N)  ,EVI  <N)  ,01  <N)  ,D2  (N)  , 


FOR  N AT (/// 
DO  2  I-1.N 


2BH  TF  DENOHINAT 


IFU  4.  /—!«#• 

WRITE  ft,  102)  ETRII)  .E7I(I1 
FOP  NAT  (/.2N.3H  (,F13.6,WH| 


101 
2 

102 

BETURN 
110  WPITE  (6,9000) 

9000  FOR  NAY  { '  FAILOPI  IN  HCR2,  C 
100  RETURN 
END 

SUBROUTINE  ZEBUS  (Ml  ,K  2,  IFDF 


GH.D1) 

A  A,  02) 

AA.D2.SC) 

,:VB,E»I,3C,IEBR) 

Dl.N.SC) 

ob  eigenvalues:) 

♦  J  (,  FI  3.  6,  1  H)  ) 

ALCULATISi  POLES') 

Wr  S,  NP.,  A,  A  A.N,B,L,C,D,  SB,  CC,  C?  ,  EVP  ,  EWI 
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D  1-  D  2f  EPS) 

ISPLICIT  SE1L*>8  (A-H.O-Z) 
DIB  Eh  SION  A  fM  ,»»  ,AA  (K,N|  , 

.  eirini  .EvlfNl  ,Dt<S), 


a  {  » .  Ml  ,C  (L,  8)  ,D(L.B)  ,BB(N),CC(N)  ,C?(N). 


*  E7R  (NI.EYIfN)  ,Dl  (S)  .  D 
DOUBLE  PRECISION  SCL.DABS 
DO  1  I*1,N 
BB  ( I)  •  B(T.KI) 

CC(I  »  C (K 2*  I)  * 

DO  1  J-1,9 

1  AAfl.J) -Ifl.JI 

KSrTE'6.10  1)  K1.K2 

101  FOR  B  A  T  (  ///  -  1 7  H  T?  FOR  ISPOT  NO.  .I3.1SH  AND  OUTPUT  NO.  ,13, 'j') 
IF( IFDFW  . EO-  0)  GO  TO  2 
H-  D(K2.K1) 

IF(DABS(H)  -LE.SPS)  GOTO  2 
JJ-N 
GO  TO  5 

2  N»=N-1 
DO  3  1-1. Mi 
H-SCL  IN,  BB.CC) 

CALL  CCONP  (N.NI>.  AA.CC,  CP) 

IFIDABS  (H»  .GT.EPS)  GO  TO  « 

3  CONTINUE 
H-SCL  IN.  BB.CC) 

WRITERS,  10 2 1  a 

102  FOR  BAT  (//,  SI,  26HNO  FINITE  ZEROS. 

GO  TO  100 

a  JJiN-I 

5  WRITE  (6.  103)  JJ.H 

103  PORHAT  (/, 31, 20H  ORDER  OF  NOBEBATOg  *,I3.9X,8HTF  3A  I H*  .  FI  3  .  S) 
CALL  ACOMPfN.NB.AA,  BB.CC, H) 

Qf.  Aftflftulu 


IF  GA  JN«,F13.t>) 


CALL  BALANC  (HR.  N,  A  A  ,  LOH  .  IHI  GH  ,D  1) 
CALL  0H7HF.S(Na,  N.LOS,  IHIGH,  AA.D2) 
CALL  HOB  (NB.N.Loh.IKiGH.  AA,  EVE,  EVI.2 


F  i.  .u,(  .niun.  nft 

IF ( IEBR  -HE.  6)  GO  TO  110 


„  „  ..  ■  >  IE  RR) 

0)  GC  - 

ACO-9-t** 

WRIT?  (6 , 109) 

104  F0RBAT(/,3X,S7HNUBERAT0P  El  GENT  ALOES  (INCLUDING  EXTRANEOUS  ZERO  » 
1LUES)  :) 

DO  6  1*1. N 

6  NPITEJfe.lOS)  EVR(I)  ,EVI  ( I) 

325  roa  riA-f  .•  C,?l3.b,') ♦Jf,P13.6.*|  •) 

100  RETURN 

110  WRITE  (6. 9000) 

9000  FO‘>HAT(«  FAILURE  IN  HQS  CALCULATING  TRANSFER  FUNCTION  ZEROES') 
RETURN 
END 

SUBROUTINE  ACOBP(N.Nr.,A.a,C,H) 

REAL'R  A-b.C.N 
D IN  £ !I5 1 0 H  1  (Kfl.N)  ,B(K)  ,C(NI 
DO  1  I* 1 , N 
DO  1  J-1.S 

1  Ajl^J^-A  (I,  J) -S  (I)»C(J) /H 
END 

SUBROUTINE  CCOBPIN.NB.A.C.CC) 

PFAL'-S  A,C,CC 

DIBEUSION  A  (NB.  N)  ,  C  (N)  ,CC(N) 

DO  1  I-1.N 
CC(  I)  -0. 

DO  1  J*  1  ,N 

1  CC  (  I)  *CC  (I)  *C(j)*A(j,I) 

DO  2  I-1.N 

2  CII)  -CC  (I) 

RETURN 

END 

rtSCTION  ICLCN,  B,C) 

R  EA  Lr  8  8  .C  ,  5C L 
DIREHSION  B  (N)  ,  c  (N) 

SCL'O. 

00  1  1-1, I 


uuu 


1  SCL  =  SCL*C(I)  PB(I) 

RET  ORN 
ESC 

DIHESSIOB  JC*  (»>  ,B8  {*,8}  ,CS  (t-.SJ  ,PR  <S)  ,  PI  (H)  ,  BES  (K)  ,  SB  (K)  ,CC  (  S)  , 

DATA  SS/8HasiS[B*T/,ai/8a  a/,B2/8HEIP  (AftTJ  / 

DAT*  2ERO/0.D0/.T  VS8“T»»/,  BLAHK/BB  /,CS/8H*COS  (B*T/ 

DATA  ED/  1R)  / 

TEBP0RA3I  BOD  TILL  JCf  IS  CALCDLATED 
DO  5  1=1, S 
5  JCP  (11=0 
TEMPORARY  HOD 

IF(T?T  .10.  1)  3?ITE(6, 90331 

DO  FOa  HAT  (//.  3X.BESID0ES  AT  THE  POLES :  *  /,  ?  1 6  .  «  P  0  L  E  S' 
a  'R  E  S  I  D  U  E  S'  ,/,X9,  'REAMAl  ■  ,  X  ,  '  IRAG  (3|  ■) 

DO  10  1  =  1, ■ 
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131  GO  TO  203 
RESIDUES  AND  PRINT  BOTH 


1=0 
1=1*1 
IP  ~ 

IF 
IF 

COKPUT 

REi  (I)  »  CC(I1  =  9B(I)  *  CC(I  *1)»BB  (1*1) 

RE5  jl*1)  =CC  (I)  *86  T*U  -  --  (I*1)»BB(I$ 

IF(IPT  .  EJ.  5)  GO  TO  HlO  ' 

pRf(i)»  eCash 
PBrj2)  =  R2 

0 .  D0|  PAT  (21  =  BLASK 

PR  (II  .PI  (II  .RES  (I)  ,  (PRT  (3)  ,0=1, B) 
PS  (I)  ,  PI  (II,  RES  (II  .  (PRT  (J)  ,J*1.U| 


COB  PL  El  POLE  AND  PRIST  JUT  ALL  FOUR 

RES  (I|  »CC  (Il»se  (II  *CC  (I*  11  »  BS  (I*  11  *CT  (I*  21  -sa  (j.2)  »cc  (I«  31  '9R  (1*1 
REE  (I  =11  =CC  <11  =  66  (I*  11  -CC(I  ♦!)  ‘  63(11  *CC  (I*  2)  *65(1*31  -Cc  (I  •  3i  » 


irjPfcHS  |PR  -ST-  1*0-10)  GO  TO  33} 


330  PR 

?rt 

BRI 


PRT  jll*  EL  A  SI 


???  3 :« 


5LA1K 


(3)*CS 

M*ED 

_ts  (6,90  20)  ?s (i)  .Pirn  . a  es  m  ,  (pet  (Ji  •  J-i,<*) 

902  0  P0a«AT(/,4I,'  (•.F13.6.M  ♦J(*.F13.6,M  •  .it,*  ('  .P13.6,  M  «  .3A8.A1) 

90  30  F08!UT]/,4X,*  (•  .PIS-E,*)  +J(*,F13.6.M  (•.?13-6,,i  •.A4.I2.2X, 

•  2  AS,  A1 ) 


*.»,A1) 

PST  (3)*5S 
1*1  +  1 

BRITE  (6,902  0)  PR  (I)  ,P  I  (I)  ,  *  ES  (I >  ,  (PRI  (J)  ,J=1 ,  4) 
PBT  [1)*T1 
PET  j2(*82 

IF(  DABS  (?8  (I)  )  . LT.  1.0-13)  PHT(2)*SLASK 

PET  (3)  *  C5 


1*1  +  1 
WRITE  (6,9030| 
PET  (3) *Si 
1*1  +  1 

BRITS  (6.90  30) 
GO  TO  100 


PR  (I),  PI  (I)  ,  F*S(I)  ,  PBT  (1)  ,lt,  (PRT(J)  .J-2.4) 
PR  (I)  .  PI  (I)  ,  RES  (I)  ,?RT  (1)  .It,  (PRT(J)  .3*2,4) 


4 

420 


3*0  I  -  I  ♦  3 
GO  TO  100 

COHPOTE  REPEATED  REAL  POLE  RESIDUE  A  SO  PE  1ST  OUT  ALL  K  OF  THER 
»00  CORTINUE 
KT*  I  +  K-1 
NS*  0 

DO  420  J-I.KT 
SH=  NS  +  1 
EES  (Jl-IERO 
DO  410  JJ-J.KT 

10  RES  (J)-RES(J)  +BB(JJ)  ‘  CC(JJ-  SS  +  1) 

20  COsilliUE 

IF  ( IPT  .EQ.  0)  GO  TO  440 
SN«  0 

PRT ( 1  -T1 
PRT  (2  *R2 
PRT  (3  *  CLA  NK 
PRT  (4  =  BLAMK 
DO  4  30  J*I,KT 

WRITE  (6 , 90  30)  PS(JJ  ,?I(J).IES(J)  ,?RT(1)  ,  *3,  (PRT(JJ)  ,JJ  =  2,4) 
430  NK*RB  +  1 

GO  TO  130 
440  I  «  KT 

GO  TO  100 
500  CORD  SUE 
RETURS 
EDO 


SUB  RO  DTI  SE  BALA  SC  (SR,  S,  A  ,  LD  B  ,  IG  H  ,  SC  ALE) 

INTEGER  I,J,lt,L,H,  X.JJ,  Nfl.IGH.LOW.IEXC 

REA  IPS  A  (SI*  Nt  .SCALE  (HI 

REAL»B  C,F.5,R. 3, 82 ,F ADIX 

REA  L*  S  DAOS 

LOGICAL  ROCGSS 

DAI  A  PADXX/Z4  21  000C00  000033  0/ 


52  * 

K  •  1 
L  *  S 
r.o  TO  130 


ADIX  *  RADIX 


2  0  SCALE  (Ml  • 
If  (J  .  cQ. 

DO  30  I 


IS-LISE  PROCEDURE  FOR  FOB  ASD 
COLORS  EXCHASGE 
J 

3)  GO  TO  5  0 


30  I  *  1,  L 
T  -  A  (I,  J) 
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aii,j  *  a  (i. a 

Ajl.Hj  •  F 
30  COHTIHOE 

DO  40  I  «  K.  a 

r  *  »  (J.  t) 

-  MS*D 

AJH,I)  «  F 
40  CCITIHDE 

50  GO  TO  (80,130),  I2XC 

::::::::::  SFABCH  foe  boss  isolatihg  ah  eigekvaluE 

AK D  PUSH  THEM  DO B I  :::::::::: 

80  IF  (L  .  ZO.  1)  GO  TO  280 
L  *  L  -  1 

FOB  J*l  STEP  -1  OHIIL  1  D3  -- 
100  DO  120  JJ  »  1,  L 
J  »  L  ♦  1  -  JJ 

DO  110  I  »  1.  L 

IF  (I  .EO.  J)  GO  TO  1  10 
ir  (A  ( J , I )  .IE.  O.ODO)  GO  TO  120 
110  CC  HTIMUE 

H  «  t 

I  ETC  *  1 
GO  TO  20 
120  COHTIHOE 

GO  TO  140 

::::::::::  szapch  pop  colours  isolatihg  ah  eigestalue 

AMD  PUSH  THEH  LE  FT  :::::::::: 

130  K  *  K  ♦  1 

140  00  170  J  *  K,  L 

CO  150  I  »  It.  L 

IF  {I  .  EQ.  J)  GO  TO  1  50 
IP  (A  (t ,  J)  .NE.  O.ODO)  CO  TO  110 
150  COHTIHOE 

f.  «  I 
IEXC  ■  2 
C.O  TO  20 
170  COHTIHOE 

::::::::::  hop  balance  the  so&hatbix  ih  rows  h  to  l 

DO  180  I  *  It,  L 
180  SCALE(X)  *  1.0D0 

HEPATITE  U)OP  for  horh  reductiOh  ::::::::: 
190  HOCCHT  -  .FAL5E- 

DO  270  t  •  K,  l 
C  *  O.ODO 
R  «  O.ODO 

DO  200  J  •  K,  l 

IF  (J  .EO.  JJ  GO  TO  2  00 
C  »  C  ♦  5  AbS  (  A  (J,  II I 
S  -  P  ♦  DA8S(A(I,jJl 
200  COHTIHOE 

::::::::::  GUARD  AGAJHST  IZ  RO  t  01  I  DOE  TO  USCEPFLOU 
ir  1C  .ED.  0.500  .08.  R  .Eg.  3.3  0))  GO  TO  273 
G  *  8  f  RADIX 
F  •  1.OD0 
E  «  C  ♦  R 

210  IE  (C  .GE.  G J  GO  TO  220 
f  «  F  »  RADIX 
C  »  C  »  82 
GO  TO  210 

220  O  ■  F  »  RADIX 
230  IT  (C  .  LT.  G)  GO  TO  20 
T  •  T  /  RADII 


C 


uuo 


240 

250 

260 

270 

280 


C  »  C  /  B2 
GO  TO  230 

::::::::  NOB  BALij.CE 
XT  (1C  *  «)  /  P  -GE.  0.9  5D0  *  S)  3 
G  «  1.0D0  /  T 
SCALE  fl|  *  SCALE  <Z>  *  t 
NOCGNV  »  -TR0E. 


0  TO  270 


DO  259  J 
A (I.J)  * 

CO  260  J 
A(J.I)  - 


«  K,  * 

A  (I.J)  •  G 

=»  J,  L 
A(J,I|  "  F 


COHTINOE 

IF  (NOCONT)  GO  TO  190 

LOB  *  K 
IGH  «  l 
RETURN 


END 


LAST  CAB D  OT  SAL  ARC 
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SUBROUTINE  0R7HES  (NS.  N.LDH.  IGH.A.OiT) 

INTEGER  I,  J.8-N  ,11,  JJ.LA.HP.NH,  IGH.KP1  ,LON 
REA L*  8  A  (»n,sf  .  osr  (IGH) 

REAL* 8  F, 3. H, SCALE 
REAL'' 8  DS3BI.0ABS.DSIGN 

LA  -  IGH  -  1 
KP1  »  LOS  ♦  1 

IF  (LA  .LT.  KP11  GO  TO  200 

DO  180  It  -  API.  LA 
F  =  0.300 

crt (H)  »  a.aoo 

SCALE  *  0.000 

::::::::::  SCALE  COLUfl*  (ALGOL  TOL  THEN  NOT  N SIDED) 
DO  90  I  *  n.  IGH 
SCALE  *  SCALE  *  DA  ES  (  A  (I  ,  11-1)  ) 


100 


no 


120 

no 


I*  (SCALE  .IQ.  0.000)  33  TO  180 
"r  *  H  ♦  igh 

:::::::  FOR  I»IGH  STEP  -1  UNTIL  H 
DO  >00  II  -  H.  IGH 
L  *  SP  -  II 

OST(I)  *  Atl.N-l)  /  SCALE 

h  «  h  »  obt  (ti  *  oar(*f 

CONTLNDE 


3  *•  -DSIGN  (OSCRT  <H)  ,3?T(  K|  ) 

H  «  H  -  ORTJHI  *  G 
ORT(Fi)  «  ORT  (N)  -  G 

FORI  (I-(U*UT)/H)  *  A  : : 
DO  UO  J  »  N,  * 

F  *  0.300 

::::::::  lOR  I*IGH  STEP  -1  UNTIL 
DO  no  II  *  1.  IGH 
I  *  IP  -  II 

r  -  r  *  opt(I)  •  a  (i.j) 
CONTINUE 


30  — 


H  DO  -- 


F  -  F  /  H 

DO  120  t  «  S.  IGK 
A  ( I.J)  -  A(I.J)  -  r  >  OS.T  (I) 

CO  NT  I  NO  S 
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::::::::::  FOSS  (I- (UrOl  1  /H)  51*  (I- (0‘  UT> ,  H) 

30  160  I  •  1,  IGH 
F  »  O-ODO 

::::::::::  FOE  J-IGH  STEP  -1  UNTIL  8  DO  --  :::: 
do  mo  JJ  *  n,  I3B 
J  ®  HP  -  JO 
F 


140 

CONTINUE 

f  *  f  /  a 

ISO 

DO  150  J  •  S, 
A(I»J)  «  4(1, 

IGH 

J)  -  F 

160 

CONTI  HUE 

180 

OET(H)  »  SCALE  * 
A  (H.fl-1)  *  SCALE 

CONTINUE 

OFT  «»•) 

•  G 

ICO 

RETURN 

LAST  CARD  Of  ORTHES 


SUBROUTINE  C8TPAN  (Nil,  N.LOW,  IGH,  A.ORT.Z) 

INTEGER  I.  J,  N.  KL,HH,SP,  NH,  T GH , LO W , N PI 
REAL*  8  A  (l)H,  IGH)  ,  ORT(IGH)  .1  (NS,  N) 

REA  If  8  C 

:  1  fO  IDENTITY  SATRIX 


HL  -  IGH  -  LOW  -  1 
IF  (HI  .LT.  1)  GO  TO  200 

::::::::::  rot  sr»IGH-i  STEF  -1  UNTIL  low+1  Do 
30  luO  NH  «  1,  KL 
HP  -  IGH  -  HN 

ir  (A  (np,HP- 1 )  .EO.  0,0 DO)  GO  TO  mO 
RP1  a  HP  •  1 


do  no  i  •  sp,  I3H 

G  «  G  ♦  ORT  (if  r  l  (!,  J1 

DIWISOR  CiiOW  IS  NEGATIVE  Of  H  FOPHED  IN  CRTHES. 
DOUBLE  DIVISION  AVOIDS  POSSIBLE  UNDERFLOW  :::::: 

G  •  (G  /  ori  (HP))  /  Air.r,  sp-i» 


SUBROUTINE  HQR2(Sfl,!',LO''»I;H*H'SB'''1' 

imijeb  |gg*fiJ'fe5:lF5ii«Sa:Ki|"'"k  V5*N’- 

'mi  ss;i;^Si&s‘Ci5!St'A,!  •  ■ 

INTEGER  BIRO 
LOGIC AL  not  las 
COMPLEX'  16  Z3  r 
CG2  PL  EXr  ^6  IX.WPL* 

aE4LT?.1???XsiAT^BEHT  FUNCTIONS  ESXBLE^ T1TR  .  "ICS  OF  REAL  AND 

inUiiiABT*  PARTS  or  OOUaLE  P  RECIS  I°R  '  -  1  RUBBERS 

SSSia  Sll  *.  (0.3D0.-1-0D0,  •  Z3 

BXTA  HACHEP/E3*  10  0  0  00  000  00  3  00/ 

T  FR  R  ■  0 
NOBB  -  0-0D0 

?  STORE  ROOTS  ISOLATED  BT  ?IIA*C 

- .  and  COB  pur  E  ^TRIX  NUW  *'•••’•  :  : : 

DO  50  I  -  1#  H 

DO  UO  J  *  K  #  ,u  ,r  t|| 

1*0  NOBB  -  NORA  ♦  D*3b  (H  ix#  J  1  1 

5r“(i  -CE.  LOS  .AND.  1  -tE.  IGH)  10  TO  ‘7 
IIP  (I)  »  H  (I#  I) 

Si  111  «  o.o5o 

so  CONTINUE 
Ell  .  JGH 

T*  0-°??.  ,rHC„  -OR  RE XI  EIGENVALUES, 

*0  ii:UiT £o.t  go  to  3*o 

ITS  ■  0  _  i 

70  DO  BO  LL  *  LOR.  E» 

(L-l‘  L-in°*1o”BStH<L‘1-'  » 

™ ... 

80  CORTInOs  ;  FOgS  SHIPT 
100  G0  T0  270 
l  ;  U  Jer*  s  a!  • 

Vr  JO  JO  1009^  ,;l  To 

IP.  <?T?.  :?ErO»n  EXCEPTIONAL  SHIFT 

C  T* » " T  ♦ " X 

c  00  izo  :  *  lor.  er 

120  1UI.II  *  M  <1*  1‘  1 

C  s  ■  D*BS(H(F*.R»n  *  0AES(H(RA,EXa2U 

J  .  :.7B00  *  s 

J  S  5o.«375D0  c  s  *  S 

130  ITS  •  ITS  +  » 
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on  o  n  non 


DO 


:::::::  LOOK  TOE  INO  CONSECnTITE  SHELL 
S3B-DIAGOSAL  ELEMENTS. 

POE  «*EN-2  ST  EP  ONTIL  L  DO  — 
140  n9  »  L ,  ENB2 
S  «  EHS2  ♦  L  -  SB 
ZZ  =  H(H,S) 
a  •  i  -  zz 

5  »  T  -  ZZ 

P  -  13  »  S  -  #)  /  H(S*1,S)  »  (((SrS-H) 

o  »  s  («♦  i,  a*  ii  -  22  -  a  -  s 

R  -  H  (S»2.a*1 

S  *  DABS(P)  »  DA  33  (Q)  •  DAES(B) 

/  s 
/  s 

IP  (fi  .  Eg.  L)  00  TO  150 


8 : 8 


IF  i  DABS  (H  (R. 8-1)1  *  (OASSfQI  * 
"  DABS  [H  (S-  1,S-1  )  )  ♦  D  APS  (ZZ) 


DAB5  (R)  )  .LE. 
DABS (H  (S 


140  CONTI  SUE 

150  NP2  »  H  ♦  2 

DO  160  I  »  RP2.  EM 
HiI.I-21  *  O.3D0 
IP  (I  .zQ.  fl  P2)  00  TO  160 
HJI.I-3)  «  O.ODO 
160  CONTINUE 

DOUBLE  QA  STEP  INV0L7ING  ROMS  L  TO  SN 
cold  sms  a  TO  EN  :::::::::: 
do  260  K  *  n,  NA 

NOTLAS  «  X  .  NE.  SA 
IP  IK  -Eg.  S)  GO  TO  170 
P  «  H  (F  5- 1 ) 

Q  -  HiK»1,K-1) 

R  ■  O.ODO 

IP  (NOTLAS)  R  *  HJK*2,K-1) 

X  -  CABSIP)  »  DA  ES  (Q)  •  DABS  (B) 

IP  (X  .EO.  O.ODO)  GO  TD  260 
P  »  P  /  X 

8 : 8  fi 

5  *  DSIGN(DSQhT(P*’P*0*g*R-,R)  ,P) 

IF  (K  .  EQ.  B)  GO  TO  180 
HIK.K-1I  *  -S  •  X 
<id  Tn  i^o 

-  H)  H(K,K-1|  «  -H(K,X-1| 


..  BACHEP  f  DAE  S 
(S*1,n*1)  )  )  )  GO  TO 


AND 


170 


ISO 

190 


ir 

p 

x 

T 

ZZ 


(L  .  NE. 
i‘  *  S 
P  /  S 

g  /  s 
P  /  s 
■  Q  /  p 
-  A  /  P 


RON  nODIPI CATION 
D3  210  J  -  K,  N 


P  *  H  (K.jf  ♦  0  »  H  (K*  1.  Jl 
IP  (.NOT.  NOTLAS)  GO  TO  200 
P  *  P  ♦  R  "  H  (K»2.  Jl 


200 

210 


H  (F*2,J) 
h  (r  - 


(p» i.Jl 

HfK.J)  i 
CONTINUE 


i  (K*2.  Jl 
•  H  (K*  2,  J)  -  P  » 
■  H(K»1,j|  -  P  * 

-  -  -  )>  »  x 


H(K,  J) 


*  T 


DO 


220 

230 


r.INO  (EM,K‘J) 

:::  COLDS*  SODIPICATION 
1.  J 

♦  I  *  NII.M-ll 

_ AS)  GO  TO  220 

P  «  P  »  ZZ  »  M  (I.X»2l 

“  S‘2)  -PAR 
*♦11  -  P  *  Q 


2  JO 
P 


CD  NT 


P  *  X  *  H  (1 .61  ♦ 

IP  (.NOT.  XOTlAS) 
P  «  P  *  ZZ  »  M  (I 
H(I,K*2I  ■  H(I.> 
M(I,K*1)  «  H  (  I  ,  I 
H|ljM  -  N(I.E) 
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\ 

\ 


::::::::  ACCOSOLATE  T3ANS  PORHATIONS 
DO  250  I  *  LOB,  IGE 

P  -  I  *  Z  <I,K)  ♦  I  *  2  (I.  K*1) 

IF  (.SOT.  KOTLAS)  GO  TO  iUO 
E  «  P  +  ZZ  »  Z(I,*»2| 

2  (I,  K* 2)  -  Z  (I.  K*2)  -  P  *  8 
ZiI,K*1)  »  Z(I.K*li  -  P  *  0 
Zfl.KJ  *  2(1,10  -  P 
COST!  HUE 


260  COSTIHUE 
GO  TO  70 


OH  E  HOOT  FOUND  : 


H  /  £S 


H(EN.EN)  »  X  ♦  T 
HR(£S)  -  H (EN.SM) 

HI (ESI  -  0.005 

ES  «  ili 
GO  TO  60 

::::::::::  TWO  ROOTS  fouhd  :::::::::: 
r  «  JT  -  X)  /  2. 0D0 
0  *  P  •  P  ♦  H 

Rm.BPc&‘5*4q,» 

I  *  H  (EN,2N) 

H[NA,llA)  *  T  ♦  T 
IF  (Q  .  LT.  O.ODO)  no  TO  323 
REAL  PAIR 
ZZ  -  P  ♦  DSIGN(2Z,P) 

KR(NA)  «  X  ♦  ZZ 

V  R(  ENi  -  HR  (HA) 

IF  (ZZ  .HE.  O.ODO)  HE  (EH)  a  X  -  H  /  £( 

V  t  (  H  A)  •  O.ODO 
wiJeh)  •  O.ODO 
i  ■  H  (FK.SA) 

S  •  DABS(X)  *  OABS(ZZ) 

P  -  X  /  S 

0  *  ZZ  /  s 

£  »  DSQRT(r^P»Q*0) 

Q  *  Q  /  H 

::::::::::  ROW  hodificatios 

no  290  J  -  RA.  H 
ZZ  »  H(NA,J) 

U(NA,J)  »  i  *  ZZ  ♦  P  *  HIFS.Jl 
H(ES,0  »  0  *  H(EN,J|  -  P  7  ZZ 
CONTINUE 

::::::::::  COLUNN  HODIFICAriON 
DO  300  I  *  1.  EH 
ZZ  -  H'l.HA) 

H(I,NA)  a  0  •  ZZ  ♦  P  '  HJI.EN) 

Mil,  Eli)  *  5  •  H(I.ES)  *  P  f  ZZ 
CONTINUE 

::::::::::  accumulate  trahs  pobhatIons 

DO  310  I  •  LOW,  I GH 
ZZ  -  Z  ( I  ,  H  A) 

Z  ( I ,  H A)  •  y  •  ZZ  *  P  >  ZJI.ZNI 
Zjl,  EH  a  a  a  i  (i  eh)  -  p  f  li 
CONTINUE 


GO 

TO 

330 

- ; 

P.PLFX  PAIR 

320 

hr] 

H  A  ] 

*  l  ♦ 

P 

UR  { 

EH 

-  X  ♦ 

? 

HI  I 

HA 

-  ZZ 

wi  i 

£1 

*  -ZZ 

3  30 

EH 

■ 

ENS2 

GO 

TO 

60 

ALL  ROOTS  POUND.  3ACESUfa3  Ttrilra  -n  ,!.n 
VECTORS  OP  OP, -EH  TRIANGULAR  ROl.a  IT....?. 

IF  (HOPS  .SO.  O.ODO)  GO  TO  1001  1  . 

fob  ih*h  STEP  -1  UNTIL  i  dd  --  . 
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DO 


600 


610 

620 

630 

6HO 


C 

C 


650 

700 


710 

720 

730 


800  H*  -  1.  * 

EH  *  H  *  1  -  K» 

£>  <=  SPIES) 

q  -  siUn 

: !!  J?l * 7iS^‘®SCl5S0 : 8 : :  :::::: 

SltSS.^rS’-i  "mi  ■ 
15 :  it  “ 

tf  =  H  jl.IJ,  -  t 

!f“ib  .ft!.  «M  =0  TO  620 

8V1V h*(iV*  »|3.») 

IP  (HI  (I)  .GE.  0.0 00)  GO  TO  63  D 
ZZ  *  H 
S  *  R 
GO  TO  700 

IF*  (HI  (I)  -BE-  0.0001  GO  TO  6*0 

If*(H  .EQ.  0.000)  T  *  BACHEP  * 
Hd.ESi  *  -t  /  T 

. ®?.T°cOl!e  REAL  EOUATIOHS  ::::: 


•O  R  S 


\ !  il-iH':  a  *.  r«  - " 

l/T'(p5ks*(lf  ,LE.  DABS  (Ml)  GC 


filial, Eg)  *  <-» 
go  to  700 


„  to  700 

H(I*1fEH) 
CONTINUE 


(-s 


T) 

T) 


GO 

X 

zz 


HI  4 1 \ 

7  o  50 


HI  (I) 


END  PEAL  VECTOR 
!  :::t^::QCoRPLi.X  VECTOR 

LAST  VECTOR  COEPONEHT  CK.-- 

. .  u  gen  VECTOR  BATRIX  is  TFT' 

|l)AbS(H  (ER.NAn  -LE.  DABS  ( N  ( R  1 . 


If  ll)ALS(H(ER.NA))  -IE-  DABS  ( N  l  “  a  .  -  >)  ) 

-  S(<(8ft!ft?M-  ?>  /  men.  .a, 

t6dcbpli(Q.odo  -“'“A  r!“‘  '  5-’,T“  * 


Z3 

H 

!! 


r<!  t* »>~.;nart  so  that 

v  U  l.A  k 

~M)1  GO' TO  720 


, _ I,  -H(HA,  EN| ) 

N  A,  SA  -  DHEALltS) 

XA'EN  ■  DIE  AG  \z  3j 
EN  r  NA  •  0.  000 
EH  '  z*  *  1  -  0D0 

!’W^=  NR  2*.  Eol  01  GO  TO  8CC 
. .  JrC'SoR  i-E*-2  srsr  -i  until 
56'790  II  -  1,  ENS2 
I  •  NA  -  II 
V  -  "(1.11  *  P 
RA  *  fl.ODO 
SA  >  HU.EN) 


/  ZZ-  TLX  (!t  (NA.NAl  -P.0: 


If 


DO 


760 


OO  760  J  * 
RA  -  PA 
5  A  ■  SA 
CONTINOE 

IP  (HI(I! 

*  h 


N ,  NA 
.  Hll.J)  * 
.  H(lij)  * 

0.0071 


H  (J  ,  RA) 

H  (3,  IN) 

GO  TO  7- 


86 


nnn  n  nn  none,  non  o  o  on 


770 


780 


R  »  HA 
S  -  5A 

GO  TO  790 
S  *  I 

IF  (RI(I)  .HE.  a. 033)  GO  TO  780 
23  -  DCHPLX(-RA,-SA|  /  DCKPLX(N.g) 
DSEAL(Z3) 

DIM  AG  (23) 

?90 

50  LV  E  COMPLEX  S3  UATIONS  ::t:: 


H  (I,  Hi) 

SO  .  _  . 

X  «  H  (1,1*1) 

T  -  H  (1*1,1 
VR  «  (UR  (I)  -  P)  »  r< 
VI  *  jURjl)  -  P)  *  2. 
IF  (VR  .EO.  0.0 DO  .A* 


HR  (I)  -  P 

. odo  *  g 

(»S  .Eo'.  O.ObO  .AND.  VI  .SO.  0.000)  VR  =  RACHEP 
•*  (OABSfO)  ♦  DABS  (31  ♦  3ABS(X)  ♦  DASS(T)  ♦  DABS(IZ)) 
23  *  DCflPLX  (XPF-2Z»*A*Q»SA,X>S'ZZ»SA-g'»I!l)  /  DCSPLXjV 
H(I.NA)  «  ORE  XL  (23) 

HJI,  EHi  •  DIRAG  (23) 

IF  (DABS(X)  .LE.  DABS  (22)  *  OABS(Q)}  GO  TO  785 

H  (X*  1  ,N  A]  ■  (-RA  -  H  o  H  ( X ,  H  A)  »  5  *  H(I,EII)  /  X 
H(I*1.EHl  *  i-SA  -  N  *  -  Q  S  H(l,NA)J  /  I 

GO  TO  790 


HI(I)  *  W  I  (I) 


Q  * 

4 


7  85 
790 


G 

23 


-  DCfIPLX(-R*r»H!I,  NA),-S-T»K(I,EH)) 
HII*1.HA)  *  DP.EAimf 
H(X*1,Sa(  *  D1K  AG  (23) 

CO HT I  HOE 

C  ::::::::::  END  COMPLEX  VECTOR  :::::::::: 

800  COHTIHUE 

EHO  BACA  SDBSTIT  UTION. 

VECTORS  OP  ISOLATED  SOOTS  :::::::: 
DO  890  I  ■  1,  H 

IF  (1  .GE.  LOH  .AND.  I  .18.  IGH)  GO  TO  840 

DO  820  J  «  I,  a 
2(t,J)  -  H  (X, J) 


/  DCRPLX  (22,0) 


820 

840  COHTINOE 


DO  880  JJ  «  LOH ,  N 
J  ■  N  ♦  LOR  -  JJ 
H  «  NINO  (J. IGH) 


DO  I.TIPLT  BT  TRANSFORMATION  MATRIX  TO 
VECTORS  OF  ORIGINAL  FULL  H  AT  Hi  X. 
for  j.h  STEP  -i  until  LOR  DO  --  ::::: 


!IVE 


DO  880 


I  -  LOW, 
0.  ODO 


IGH 


860 


DO  860  N  «  LOW.  H 
ZZ  ■  22  ♦  2  (I  , K)  * 


H  (R,J) 


2^I,J) 


880  CONTI  HO 

GO  TO  1001 


22 


1090  IERR  •  EH 
100  1  RETURN 


GET  ERROR  —  NO  CONVERGENCE  TO  AN 
EIGENVALUE  AFTER  30  ITERATIONS  :  :  : 


END 


LAST  CARD  OF  H3R2 


SUBROUTINE  2ALBAK  (NR,  N  ,LOil ,  IGH,  SCALE,  S,  Z) 

I  NT  EG  FR  I,  J.  K.R,  N,  1 1,  NR,  IGH  ,  LON 
°FAL»8  SCALE(H)  ,2{Hr.,  N) 

REAL" 6  S 

IF 

IF 


(H  .EQ.  0)  GO  TO  200 
(IGH  .  EO.  LON)  GO  TO  120 


g 

NO  R“ 

5,  VI) 


87 


LMJ'J  u  uu  u  u  u  u  ouu  u  u  u  uu  u  u  u  u  uo 


DO  110  I  *  LON.  IGH 
S  »  SCALE(I). 

::::::::::  LEFT  HAND  EIGEKT  ECTO  RS  are  sack  tpamsFORNED 
IF  THE  F0REG0IN3  STATEHENT  IS  REPLACED  BT 
S«1.0D0/SCALE  tl|  .  :::::::::: 
so  100  j  a  i  n 
100  Z  (I,J)  «  Z  (I.J)  *  s 

110  COM TXNOE 

FOB  I-LOW-1  STEP  -1  UNTIL  1. 

153*1  STEP  1  UNTIL  M  00  --  :::::::::: 

120  DO  1»0  XI  »  1,  » 

I  *  IX 

IF  (I  .GE.  LOW  .AND.  I  .  LE.  IGH)  SO  TO  140 
IF  (I  .LT.  LOW)  I  «  LOW  -  II 
K  »  SCALE(I) 

IF  ( K  .  EQ.  X)  GO  TO  1U0 


130 

14  0  CONTINUE 
200  RETURN 
END . 


DO  130  J  *  1,  S 
S  »  Z  <I,J) 
Zfl.J)  -  5 
ZjK.J)  »  S 
CONTINUE 


E.JI 

«  Z(K,J) 


LAST  CARD  OP  SAL BAK 


SUBROUTINE  HQR(NH,M,LOW,ISH  #H,¥R  ,  WI  ,  I  ES  R) 


INTEGER  I,J,K.L.K,N,EN.LL.«N,MA,NN,I3*,ITS,  LOW  ,.TP2  .  ENH2  „  I  ERR 
REAL1' 8  H(*'K.NI,WR{Nl,Wi)N) 

SEAL'S  F.O.R.S.T.N.X.  T,ZZ,NOR.H,  SACHEP 
R EA L*  8  nSCRT,  DABS.  DSICR 


40 


INTEGER  n?n6 
LOGICAL  NOTLAS 

DATA  HACHEP/Z34 100000 000000  00/ 

XERR  -  0 
MORN  -  0.0  DO 
K  *  1 

store  roots  isolated  bt  balamc 
and  CONFUTE  NATRIX  NORn  ::::::: 
DO  BO  I  »  1,  N 

DO  «0  J  -  K,  N 
NOHR  *  NORN  ♦  DABS  (H  (I,  J  )  ) 


I! 

IF 


(I  .GE. 

WR  It)  -  H(I.I) 

wx  m  *  o.odo 

TI  HUE 


GE.  LOW  .AND.  I  .IE.  IGH!  10  TO  50 


50  CONTI 

EM  •  IGH 
T  »  0.000 

SEARCH  ro.f  NEAT  Z1GE  NV  ALU f S 
60  IF  / ES  .LT.  LOW)  GO  TO  1021 
ITS  '*  0 
MA  ■  EW  -  1 
E NN 2  *  HA  -  1 

LOON  FOR  SINGLE  SHALL  3D  3-  C  T  AGONAL  EL  TRENT 
FOR  L* EM  STEP  -1  CNTIL  LOW  DO  --  :::::::::: 
70  DO  80  LL  «  LOW,  EM 

L  *  TI  *  LOW  -  LL 

IP  (L  .EO.  LOW)  GO  TO  13  0 


nn  n  n  non 


r 


DABS(8(L-1,L-1))  *  D  ABS ( d  (L,  L)  ) 
(S  .  EQ.  O.ODO)  5  *  NOSH 
^DABSiH  (L.L-1J)  --E.  HACHEP  »  S) 


S 

i  r 

i  r  , 

SO  C0H7I  HUE 

Foaa  shift  :: 
A  *  H  (EH.EH1 

IF  (L  .  EQ.  EH)  00  TO  270 
T  =  H  (NA.HAl  _ 

0  *  H(EN,HA1  *  H  (NA.EH) 
ir  {L  .  EQ.  HA)  GO  TO  280 
IF  (ITS  30}  GO  TO  1 

IF  (ITS  .HE.  10  .AHD.  IT 


GO 


109 


100 


T  ■  T  ♦  I 


1000 

.  .  .  _  ITS  .  HE.  201  GO  TO 
FOBS  EXCEPTIONAL  SHIFT 


DO  120  I  a  LOU.  El 

120  8(1,1}  =  8(1,1}  -  X 


DABS  (H  (HA,  EHH2)  ) 


S  *  DABS  (H  (El, HA)  ) 

X  «  0  . 7  5  DO  •  5 

T  »  X 

H  *  -0.  B375D0  •  S  •  5 
130  ITS  *  ITS  ♦  1 

::::::::::  LOOK  FOR  TWO  COHSECOTITE  shall 
SOB-DIAGONAL  ELEMENTS. 

FOB  H*£N-2  STEP  -1  UNTIL  L  DO  .. 
DO  140  SB  «  L,  EHB2 
fl  ■  ENH2  *  L  -  SB 
22  -  H(A,B| 


/  H(B*1,  H) 

-  22  -  R  -  S 


nin.n 

x  -  « 
s  «  x  -  22 

P  -  (8  »  S  -  8) 

Q  -  H  (HU,  .VI) 
fi  -  H  (B*2.  A*  1) 

S  *  DAB5  (?)  <  DABS  (Q) 

P  •  P  /  5 

Q  -  0  /  S 

S  «  R  /  S 

IF  (B  .  EQ.  L)  GO  TO  150 
IF  (DABSiH  (r,s-1»)  S  (DABS(Q)  ♦ 
r  (DAbS  (H  ( S—  1 ,  A  -1  ))  *  DABS]-!) 


H  (S,H,  ’) 
♦  DABS  (R) 


140  CONTINUE 
150  HP2  «  A  ♦  2 

DO  160  I  •  BP 2 


DABS 
»  DA 


S’sU 


»  LS.  HACHEP  *  DABS  (PI 
(n»1,fl*1)  )  |  )  GO  TO  k6 


mu  i  -  nr^i  EH 
H  II, 1-21  *  O.ODO 
IF  (I  -EQ.  AP21  GO  TO  16  0 
H(I.I-3)  «  O.ODO 
160  CONTINUE 

DOUBLE  OR  STEP  INVOLVING  R38S  |  TQ  r»  , 
COLU.BNS  a  TO  EH  ::::::::::  TO  -*  *I,D 


do  260  A  «  a. 

S  *  A 


170 


1«0 

1  90 


HA 

notlas  »  *  .he.  ha 
IF  (A  .  EC.  B)  GO  TQ  170 
?  •  ll(E,*-H 
0  *  h|e»  1.R-1) 
a  »  o.odo 

IF  ( NOT  LAS)  I  »  HfF*2,*-1) 

I  -  DABS!?)  ♦  DABSIQ1  ♦  DAPS(P) 

IF  (I  .EC.  O.ODO)  GO  T9  260 

p  •  p  /  i 

l  l 

S  »  DS.r.N  (DSQRTJP*  Hu'?*  fi'Rl  ,  P) 

IT  (A  .  EQ-  H]  GO  TO  183 
H(R,A-»1  «  -S  •  X 
GO  TO  190 

IF  IL  -SE.  B)  H  (A,  A*  1 )  -  -H(A,X-1| 

p  •  p  *  5 


89 


nnonnonoo  n  o  on 


ZZ 

i : 


■  B  t 

Q  / 

H  / 


DO 


200 

210 


7  S 
P 
P 

BOB  10DI7I  CATION  ::::::: 
210  J  *  K,  SB 
P  *  U  [K.jf  *  Q  c  SII.I.J, 

IF  (.NOT.  NOTH  SI  GO  TO  200 
P  *  P  ♦  B  •  H(K*2.J) 

>2,i)  -  ?  *  ZZ 

'  *  *  I 


H  (K+2.J1  *  H  (K*2,3)  -  ? 
H  (K+  1 . J)  »  H  (K*  l.ji  -  P 
"  F  *  1 


COBT 

J  *  HINO(Slf,K*3^ 


ODIFICAT ION 


220 

230 

260 

210 


. ..  COLUNM 

DO  230  I  *  L,  J 

P  *  X  »  H  11.  Ki  *  I  ► 

IF  {.NOT.  NQTLAS)  GO  TO  220 
f  »  P  ♦  ZZ  P  H(I,K*2) 

H  (I.  K*2)  -  H  (I,  K*2)  -  P  *  R 
H  (I,  K*  if  *  H  (I,  K+  1)  -  ?  * 
H]^K)  >  H  ( I ,  X)  -  > 

COBTT"*- 


CONTINOE 
GO  TO  70 


ONE  ROOT  FOUND 
X  ♦  T 
0.000 


280 


320 

330 


UR(EH)  - 
UI(ENj  • 

EN  «  NA 
GO  TO  60 

::::::::::  TWO  BOOTS  FOUND 
P  -  (Y  -  X|  /  2.000 
O  ■  P  *  P  ♦  ■ 

ZZ  •  DSQRT  (DABS(Q)) 

X  ■  T  ♦  T 

IF  (0  -LT.  0.0D01  GO  TO 
::::::::::  REAL  PAIR  :  ; 

ZZ  -  P  »  DSTGli  (ZZ.  P) 

UR<NA)  •  X  *  ZZ 
HRjTHl  »  RN(NAI 
IF  (ZZ  .NE.  3. 0001 
UI(NA)  «  0.000 
U I ( EN)  -  0 . ODO 
GO  TO  330 

COMPLEX  PAIR 


323 


NR  ( EN) 


X  -  «  /  ZZ 


MR(NA)  •  X  ♦  P 
U R f  EN)  «  X  ♦  P 
WI(HA)  «  ZZ 

bi{en{  *  - zz 
EN  -  EN«2 


GO  TO  60 


1000 

1001 


IERR  - 
RETURN 


EN 


SET  ERROR  --  NO  CONY  ERGENS  E  TO  AR 
EIGENVALUE  AFTER  30  ITERATIONS  ::: 


END 


LAST  CARD  OF  K3B 


SUBROUTINE  PSDCALIt;:.  MS.  FA.  X.NC.GW.If  ,  C  ,  NO  .  H I .  H  U  .  H, 

1  rBGE,NG.GAIlrA'_L,r,NB,«t,D’  ,  D2,  JCT,  S  ES  ,  Q.  K  ,  86,  CC  ,  I IU  , 

2  IPSD.INORB) 

PS  DCAL  COMPUTES  7R  F  PSD  OF  OUTPUTS  OR  CONTROLS  OF 
A  CCNT  ROLLED  SYSTEN 


II0« 


X  PSD  •  1 

•2 


OUTPUT  PSD 
CONTROL  PSD 


PSD 

PSD  AND  TF  RESIDUES 


90 


oouuu  u  u  uyyu 


ISORH*  1.2,...  KS  NORMALIZED  3T  I7H  PROCESS  NOISE 

NG*i  ,.. .  NG*NO  NOSaALIZED  ET  IIH  REAS  NOISE 

D005LL  PRECISION  PA.I.G  «,3»  .  C.H  T  ,  H.  FBGE  ,G  A"  ,  ACL,  P.BE.UI.Dl,  D2  ,  RES 
1  BE  fCC,S,R  ,?SD,  S.DNORH.DSI  ,  'S1I ,  iLOG,  ESOD , DV , ST, OH , R E,  AI,  HO,  Dill 
coaPLErno  so, zn,  zz 

DIMENSION  PA(N2,»2)  ,X  (12,12)  ,3V  ( 12, NS)  ,  C  (NC,  NS)  ,  HI  (SC,  12)  , 

1  5  (NO, RSI  ,  Ft>GZ(NS,  NO)  ,01 R  (N  S.N3)  ,  ACL  (  »S  ,  NS)  ,  F  (NS,  NS)  ,1R  (N2)  . 

DATA  0«  1/1 1  m  .00, S.  DO, 10.  00/ 

IF  ( IT  0  .EQ.  0)  IT0*1 
IF  (IMORK  .  EQ.  3)  INOBH  -  1 
IPT  =  0 

IF  (IPSD  .GT.  1)  IPT  *  1 
II  =  I10RH  -  MG 

IF ( II  .GT.  31  WRITE  (6 ,8000)  II 
8000  FOf»RAT(/*  SUBSEQUENT  PSD  IS  NObHALIZSD  3T  REAS  NO.*, 13) 

IF(IX  .LE.  01  MBITS  (6  ,80  1 0|  INORH 

8010  FOR  HAT  (/ '  SUBSEQUENT  PSD  IS  NCR  R  A  LIE  ED  BI  PROCESS  NOISE  SO.  *,13) 
NSQ  -  Ni"S2 

COMPUTE  dGENSTSTEH  OF  CONTROLLED  STSTEtl 

::::::::::  PORH  PA 
DO  10  1*1, *S 
DO  10  J«1,NS 
F A ( I ,  J)  »  ACL  (I  ,  J) 

10  FA(NS*I,J)  ’  0.  DO 
DO  20  1*1,  NS 
DO  20  J*1,  NS 
ST  *  0.  DC 
DO  IS  1*1, NO 

IS  ST  -  ST  ♦  FRGEJT.KI  «-fl  (1C,J) 

FA  f  I  ,  NS  ♦  J)  *  -ST 
20  rA(NS*I,NS»J1  *  f(I,J)  -  ST 

CALL  RAIFST  (N2,n2,n5, 9,PA,»  ,«  [9  (U,  1PD13.6)  )  •) 

Ct«fHim'ief.eniir«»  DEBUG  ABOVE 


CALL  HQF2(1_.  N2,L0U,IH1GH,  FA,nR,MI,I.  I  ERR) 

I F  ( I  ERR  .NE.  0)  GO  +0  1006 

CALL  BALfiAK  IN  2,  H2,LOW,IHi3K  ,D1,  N2,X) 

CALL  RAFPNT  (N2 . *2, N 2. 9,1,4,  •  {?( 1  X.  1  PD  13.6)  )  ■) 
CA«nt*cr*(ic*n — **  DEBUG  AbOVE 
100  CONTINUE 

C  ::i  :::::::  DETERMINE  nODAL  MATS  ICES 

Ir(ITC  .EJ.  1)  GC  TO  130 
C  RSUBU 

DO  110  I*1,<C 
DC  1 10  J*1 , N2 

st  *  0.  ao 

DO  10S  K-l.NS 

10S  ST  *  ST  -  Cjl  ,K)  a  I  (K,  J) 

1  10  Him,  J)  -  S7 
GO  TO  150 

C  ::::::::::  HSUS  I 

1  30  DO  1*0  :*1  ,  NO 
DO  UO  J-1.M2 
ST  *  0.  DO 
DO  135  r.-!,NS 

135  ST  *  ST  ♦  H(t,K)*I(K,J)  -  H  (I,K)  *X(NS*N,  J) 

140  HT(I.J)  -  ST 

CALL  PA  FRIT  (NO, NO, 12,  8, HI, 4  ,*  (9(11,  1?D1  3.5))  *) 
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DEBQG  4 BO VE 

150  CALL  8IliV{Msq,X.B2,ST,n1  ,D2) 

CALL  RAPRMT  (Ji,  »2, R2, 9, t ,4,  1  (9  ( 1 1 , 1  P01  3 . 6)  )  ■) 
cr*»f«ui*r*»rtr^sa  DEBOG  ABOVE 
C  r :::::::::  GS’JBB 

DO  160  I >1 , >2 

S?  26USJ*"G 

DO  155  K-1.MS 

155  ST  »  ST  -  X(I,HS»K)  ‘>GAa(K,J) 

16  0  GB  ( I  ,  J)  *  ST 

CALL  RAPRMT  (6  2,  B2,  MG.  9.GS.  A  .  •  (9  ( IX,  IPC  1  3. 6))  •  ) 

DEBUG  ABOVE 

C  ::::::::::  OSE  SELECTED  SDR  RALIElTIOM 

200  irusoan  .le.  bg)  dnorh  -  i  .do/o  iiborr,  ihopsi 

XrfIKOEfl  .GT.  HG)  DRORN  *  1  .  DO/fi  (iKORR  -  NC.  I  NORR-RG) 

C  ::::::::::  OETEHRISE  BAUDS  I DTH  Of  COMTROLLeD  3VS 

ERA X  *  O.DO 
DO  210  1-1  .  M2 

EROD  -  DABS  (MR  { I)  *6 2  +UI  (II  0*2) 
iriEROD  .3T.  Efl AX)  ESAI  *  SHOD 
210  COITIKOE 


TROD  ■  DSO 
EROD»2*SRO 


RT(ERAX)  , 

ROUMD  UP  TO  BEAREST  2,4,5,8,10 


SLOG  *  DLOG 10  (EHOD) 

IFIELOC  -LT.  O.DO)  T?OH  »  - IDIBT (DABS (ELOG)  ♦ 
If  [  CLOG  -CE.  0.  DOS  IPOB  »  IDIKT (ELOJ) 

EHAX  »  EHODrtlOPP  (-IPOW) 

Iff  ERA!  -GT.  2.  DO)  ESOD  »  2. DO 


DU  -  EHAX/20.D0 

aoo  10  Poikts  3  DECADES  OP 
IffEROD  -LT.  5.0)  GO  TO  212 
ERA!  -  1.001 
IK  »  3 
GO  TO  216 
212  ERA  X  -  5. DO 
IK  •  2 

216  COM  TX  HUE 

::::::::::  SJOBE  30  PREQUERCIES 
DO  220  1-1,28 
220  B  (II  -  OB-  (1-1) 

DO  218  1-1,3 

IP  •  20  ♦  3*  (I -11 
DO  21 8  j-1,3 

IX  -  SOD  (IK-J-1.3)  •  1 

JJ  -  0 

I F  ( I K  .EQ.  2  .AND.  J  .3!.  2]  JJ-1 
B(ir»J)  »  0«1  (1X1  HO"  (  IPOB*I-1»JJ«IK-2) 
2  1  B  COMTIBUE 

II  -  ROD  (I  K  ,  31  ♦  1 

B(JO)  »  OKI  lilt)  -10-»(IP3W*3  UK-2) 

LARGE  LOOr  THRU  OUTPUTS 
irilT  J  *  PQ.  1)  ML-  SO 
If  I  ITU  .  EQ  •  2)  ML  -  »C 


LOOP  THSU  PROCESS  SDISE 


If 

II  'J  -PQ 

If 

ITO  .  EQ 

DO 

BOO  L-I 

DO  250 

FSD(I) 

DO  300 

uu  jnu  i  -  i  , 

DM  1  *  DMQRR-0  (I,  I) 

If  (  I  TU  .  EQ.  1  .AMD.  I  FT  .-3.  1|  .PITS  (6 , 8020)  I.L 
8020  POM  RAT  (/•  TPAK3PEM  PGSCTIw*  F?OS  PROCESS  BOISE1, 12 
1.’  RSASBPESEMT  ’,12) 

iplITU  .10.  2  .  AMD.  I PT  .S3.  H  MBIT- (6,8030)  I.L 
8030  PCR  RATI/’  TPAMSfEP  rUBCTIOS  TROR  PHOCtSS  BOISE  ‘,12 

i«  comtJol  ■  .121 
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IF  j;  It  a.  SO.  1)  CALL  BESID  {I.L.B2.JC*  ,  >,G,  SB,  ML.  HT.WR.NI. 

iF(*fafto?iLcii-L  BESID  (i,L.»2.;cr  .  hg.sn,  hl.  ho. he, hi. 
BSS.0B.CC.IPT) 

DO  260  t=1,?0  _  .  ... 


Z^£  DCZr'LX  (0.  DO.  0.  DO) 

Vc 


25* 


256 


IFlSlI|li)1f"2&0.25»,256 

ZD ("  dczVLk  -bS  (II )  .°5-'iI  l-T>  ) 

ZZ  *  BES  (II)  /ID  ♦  22 


ZZ  *  BES  (II)  /ID 
GO  TO  260 
BE  =  «B(II 
a  i  =  «i|n 


[l, 


ZD  =  DCBpLitFEA'P  *  Mf.t2o;-r'‘*2t-2-a:)''RE,'0"L 
Z°  *  DCftPLxln£S(II*I),»I-8:=  CL'.)*SE.RES(II)*Oh) 
ZZ  »  ZZ  ♦  ZX/ZD 


260 

280 

300 


_  -S/ZD 

PSD°{”I*UisD(X)  ♦  0«1'|ZZ*DCC*>G,z2n 
CONTINOE 

,  GSOBt 

DO  320  1-1.82 
DO  320  J-1.80 
ST  *  0.  DO 

316  g?  r!T't:,ijI.K)*FSOE(K.J)  -  X(  I.  SS*  A  »  *  r  6G!  (B  .  J) 

32  0  gMi'ftp«jfl«.I|2.S069 
CC*6<‘C«»*r.P«-n»«’;«P1DE|U|;1|gOji|Jls  ,0IS£ 
c  *  *  *  I  *  J  Vi  un 


35* 

356 

360 


if-ATf/*  TRANSFER  FUNCTION  FBOH  .1  HINT  ‘.12, 

j^IYO^Ert^t)  cJlLL^  BESID  «.  L.B2  .  JC?.  *  .■>,  GT  .  NL,  HI.  »R  ,  h  I. 
IF^itU.'io^JCALL  KESIO  (I.  L,M2,JCF.S.1,GT.HL.HU,WR.BI, 

*  K  A  *  r~r~  ▼  m»\ 


B^Cc'.IPT) 

00z|82  Vcblh O.DO.O.DO) 


FES* 

RES* 


SS  US‘ft-1, 


,lWxaiil«fe8fcg|4j58— x  <K» . 

ZZ  »  ZZ  *  Br.S  (if) /ZD 
GO  TO  360 
PE  -V  R  (I  I) 

ZD  »  DcIpLX  (REP*-  2  *  A  I-%r2  1°"**  -  2.  DO^r.E'DS) 

52  .  DC"PLX  BES  (II*1)o*I-B£S[1I)»pe,RES(II)-0S) 

ZZ  «  ZZ  •  ZB /ZD 


378 
3  80 
390 


ZZ 

CONT  I  HU  £ 

IF ( IT  0  -  EU-  *  -  OB 
BSD  (F!  *  DSO(J) 

BSOjK)  -  PSD  (M 

contisof 

CONTINUE 

IF 

IF 


I  .BE.  L)  GO  TO  378 
»  ONI 

0P1»  (ZZ"DCONJG  (ZZ)  ) 


FllTO  -EO-  1)  •*FITI(6.90D3)  \ 

F(ITO  .EG-  2)  N6ITEj(6^3013  .  i  L 
9000  rCs  BAT{/*_  BSD_OP_aUTi  U.  ,13, 


-»'r(/,'PSD  0T  a»TiU.-.w,  TORCEO  DT  all  BOISE-lPAD  FR  ED, 
BOIo’pojittu^m'oO’o^loLMS.-  rONCKO  HT  ALL  BOISE- (BAD  FBED 


400  CCS 71  NOE 


p.rrcpN  _ 

1000  CONTINUE 
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onon 


\ 

i 


CALL  EREXIT  (*2, FA. IERR} 
RET  09  ■ 

END 


SOS  RODTIRE  EREXIT  (I, A, IE  St  I 

EREXIT  RETURNS  THE  KUNeSR  0?  THE  EIGENVALUE  WHERE  HQR2 
FAILS,  THE*  STOPS  THE  PROG  RIB. 

INTEGER  IERR 
DOUBLE  PRECISION  A 
DIHENSION  AfN,*} 

WRITEJ6.9000)  IERR 

9900  FOR  MAT  ( *  FAILURE  IN  H  QR  2  ON  EIGENVALUE  NO.  •  ,  131 
CALL  RAPF.NT  (R.N  ,N,9 .  A.*,  ■  (9  (IX,  1 PD1 3.  6)  )  • ) 

STOP 

END 
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nnnnon  nnonnoono  ftononnonfiftoconcnonrinoftooooofinooo 


ccccccccrccccccccccccccccccccccccccccccccrr cc  CCCCCCCCCCC.  cc 

SEHSITIVITT  COVARIANCE  PECSEAS 

THIS  FSGGRAR  IS  USED  TO  SOLVE  THE  EE?  0-  S'tS'TIVITT 
EQUATIONS  NHEN  THFEE  IS  IN  INCOBEECr  * '  =>i?»®S^A  tt  ni 
OF  DTNAflICS  IN  THE  DE  SIG  N  Of  THE  KELT  an’  Flilf^P  pkp 
ECBAIIOSS  3ECCRE  ‘  ' 

FOOT®  (F“  -X*  9)  r+  F  (F*  -KPH)  TO  FV  *V  TOFT  03 T*»c  RKO  T 
VDOT*  FV  +  »IF:'-K»H>  TODFT-GJST 
0D0T=  FUOfT  »G  CGT 

THE  PRINCIPAL  PROGRAM  INPUTS  ABE  THE  :  LOU  T  NO  “O- 
LLECT  TON  Of  SXSTEH  AND  FILTER  3 ATSICE  S  " 


KE 


THE  INITIAL  COVARIANCE  HAT  RI I  (SI  l)t 
THE  TSUTH  MODEL  LINASICS  AIKII  ■  NX  HI 
THE  FILTES  MODEL  2TNA3ICS  HAIKlf  sxin 
THE  TBUTH  HOD  EL  MEASUREMENT  RA  T?  y I  f  LX  N\  yii 
L  IS  THE  MEASUREMENT  VECTOB  Dia'Nsfoi  ' 
THE  INPUT  NOISE  COVARIANCE  »  A  T  E  I  i  /  i  x\ 

THE  .MEASUREMENT  NOISE  COVAaiAN C*  BiOl 

PMfPE  -iril  lUfTI  ■  *  L  S  1 


WHERE 


FILTES  3 AIH  (NIL) 


X  ( LX  L ) 


rCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 


-  ccccccccccccccc 


THIS  PRCCHA3  HAS  SEEN  DEVELOPED  OStKS  "hp  T*er  *  f  rri  oy 
AVAILABLE  IN  THE  COrFOTEE  CENTER  OF  TH*  Jfiv!?  “iDP'KI 
POSTGRADUATE  SCHOOL  ” 

IMPLICIT  3£AL»B  (A-H#0-2) 

COiNON  F  <7,71  ,fS  17,7!  ,G3G?(7)  , AN  (7.21  #p  f2  1) 

XlttZ  \h t\  Wih{' 1  * tv 7’  ’  rsk*ni  l) 

C03B0!l/NTr/N,NS.tlPD 
DIMENSION  PFULL  (7,7|  ,  FSJR(7) 

DIMENSION  DDT  I  ? | 

DI3FNSIOK  U  <2  8>  /«(7,7).?(29).0D(2R)  ,VD(7,7) 

N.OHDEB  OF  THE  3VSTEM  MODEL 
HP-NUMBE?  OF  POINTS 

NPO»CONTPOL  OF  INITIAL  DIAGNOSTIC  OUTPUT 

D7»TIME  INTERVAL 

EXT  FP  HAL  FUN 
CALL  CGET:0(3fS,6) 


THE  FOLLCN I NG  SECTION  READS  THE  S?ECIri®D  INPUT 
HArRICES.F.F'.GQGT.N'H  ASB  R  -u  ‘ 


98 

97 


H  EA  D  f5,9Pl  N,  NP,  SPO,  DT 
8  FCR-AT1  JT3.F10.  S) 

NRTTE  (6-97]S_NF  NPDtDT 
7  FORMAT*'  1’  ,315,  ll.S3o.1D) 
NS«  S*  (N*  11/2 


(*♦71/2 
»'  T  2  •  3 
NV»2:»RS»A**2 


>USS«' 

NV®  i*  NS« 

99  FCF  NAT  (8  F>  0.  S) 
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onononoonnnnononooonnnoo 


V  i 

i  - 

i  Ji 

i  j 


i  • 


READ  (5,94?  (?  (I.J)  .J*  . 

CALL  CSfcFH  (*F  1  #  i  ,?*7t  **  N 


■  SHHUwtU-r... 


DO  6  1*1.105 
VAR  (lf*0. 

DO  7  Si.* 

DO  7 


IER) 


IER) 


DO  2!  1*1.7 


DO  l 

FSfIKH  (I.jf  *r?  (I  *1' 

Z 1  III ?  * i i  l  ? r2  KH f  h?  Vs«M1 .  7 .  H.  *  .  M 

ckC  t  Sis *S  ( ^ ?s-k Sf  t*  *6] Vs*  r HT,  7 . 1  > 
T*0. 

TOL  *1 ,D-5 

I  SO  *1 

IF(M.  U5.7)  GO  TO  11 

DO  31  1*1,  *S 

1,-1.  ♦  1 

J  1  VAR  (H  *U  (I| 

00  32  1*1, * 

DO  32  J*1,* 

L-L»1 

32  VAR  (L)  *V  (I,J| 

DO  33  1*1, R 
l*L*  1 

33  VAR  (L)  *t>  (I) 

11  DO  10  F*l» RP 
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nnnno  nnn  or.rto  no 


tend*  final  Tint 
TEN  D«K*DT 

d,pBR  SUBBOOT  IKE  PINOS  THE  SOLUTION  OP  THE  STSTEH  or 
Dl?  EE  RENTI  A  L  EQUATIONS 


CAUL  0tEBR(«*..-0H.T,V»B.TEHD.T0L,I»D.C,10S.Hlt.IEa) 

CALCULATE  ADD  PBI*T  THE  BBS  ESTIHATE  EPBOBS 
DO  30  I-',*  „ 

90  wafi«»W:)W!.'P 

IT  DESIRED  PRINT  THE  COVARIANCE  HATR  ICES .  P  .0  AND  T 

CALL  USwPn  |,,|’.'VtAftlH13  ’l*' 2)  K'!,'2> 

CALL  USA  SB  ( •  P  1 .  1,VAr(N11  «  ”•  *> 


crb  *-  v  - 

10  CONTINUE 
STOP 

SUBROUTINE  TTAAHX  (A,  N.NC.IA) 

inPLICII  BSAL^B  IA-h.0  1) 
DIHtNSION  A(1A,IA) 

DO  1  1-1,5 

DO  1  J-1.N 
i  B(t,J)*A(J,H 

00  2  1-1,* 

00  2  J-1,* 

2  BM«B,I,.» 

SUB  POUT  I  NE  FUN[NV,T,VAP,OFT) 


suoruui  Ant.  • 

FC*  SUBPOUTINP  is  USED  FOB  EVALUATING  EU  ACTIONS  ( I  N  Pi) . ) 


'PE7IKH  [I,  7L  ,  B  (2,  .) 


mi 

UlB 


L*0 

DO  1  1-1, *5 
L-L  ♦  1 

1  U  (I )  -  VAP  (L) 

oo  ;  i- 1 , * 

00  2  J«1.» 

L-L*  1 

2  V  (I  .  J)  -»AS  (L) 

DO  3  1-1, *3 
L*L  ♦  1 

3  :5UrEo!i^T-NPD 

iV^tJgE.SI  GO 
99  fOBnA,^(,9T•,,  D2S.  15) 


CO  TO  15 
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CALL  OSWSHJ'O’  ,  1  ,U,  K,  2) 

C1L  L  USRFB  (>*  •  ,V,  7,  K,K,2l 

CULL  USRSB  I'P',  1,V,  H.  2$ 

CALL  VBUtfS(F.O,S,S,7,rSPl.  7> 

IPJKT.LT.5)  CALL  OS  IiF  fi  ( '  FD  *  ,  2  ,  T  LP1 . 7 ,  H  ,  S  ,  2) 

CALL  VRULSF  (U.K.  FT.  K,  7.TKP2  ,7f 

IF(K?.LT.5>  CALI  OSilF  B  { ’  OPP  3,  TSP2. 7  ,  K  ,  R  ,  2J 


DO  5  1-1, » 

DO  4  J-1.R 

TBP1  (I.JJ-TRP1  ( 1 1  J]  ♦  TSP2ft,  J| 

TP.P1  (X,  X*  =T^?  1  }l ,  li  *G  CGT  (X } 

CALL  VCVTPS  JTBP1,S,7.  ilDI 

IFJKT.LT.Sl  CALL  USVSBJ'  BOB  I'  ,4 .00.  N,  2) 

CALI.  VBULFF  (P  ,  V  ,  K,  H  ,  7,  7, I BPi . 7 ,  IEB| 

IF(KT.LT.S)  CALL  0  S  4f  H  ('  PV  •  .  2,?  BP  1 ,7,  *  ,  S  .  2* 

CALL  VBULPF[V,FSBKHT,  K,H,|l,7,7-TBp5,7,!E&f 
IPJKT.LT.5)  CALL  USWFBJ'  vs]  FS-KH)  I*.  t6,  TBP2.7,  N,  H  ,2) 
CALI  7BULSF(U,H -DPT  ,R  ,7,  TBP  3,7) 

IF  (  KT  •  LT.5  )  CAL L  US  SF  h  ( '  0»D  FT  ■  ,  5,  TBP3 , 7  .  *  ,  R  ,  2) 

00  7  1-1,* 

DO  6  J-1.S 

VDJI ,  J)  -TBP1  (I.  J)  *TRP2  (I,J|  *TBP3  (I,J| 

VD 1 1  ,l{  *  VD  (I,  if  -&OGTJI) 

IF  i  KT.LT.5)  CALL  USUFE  {•  fODT*  ,4  ,VU,7,  »,  H,  21 
CALL  VBJLFSJFSr.KH.P,  H,.1,7,  T3P1  .7) 

ifjkt.lt. Si  call  usafbc*  (FS-khi  P1 .8,rapi,7,)i,s,2) 

CALL  VBULSF  |P,K.PS3KH?.H,7  TBPi  ,7f 
XFJKT.LtTS)  CALL  USUP  fl  ( 1  PJr  S-LH)  .  ,9-TRP2,7,)l,H,  2) 
CALL  VBULFF  (OF,  f  ,K,li.  M.7.7.  TBP3,7,IEat 
IF(KT.LT.S)  CALL  US  VF  F.  (' DP' 1  ‘  ,4  ,TRP3,  7  ,  K  ,  2» 

00  8  I- 1  ,K 
DO  8  J- 1,8 

TBP  1  (I.UJ-TBPI  (I,  J)  »TBP2  JI ,  Jl  »TBP3j:  .  J) 

CALL  VRULFB  (V,DF,ti.  K,  8,7,7,  TRP3, 7, 1  Eft) 

IFJKT.LT. 51  CALL  CSyPB(*»r*Or',5,TBP3,7,II,!lr2) 

DO  10  1-1,* 

DO  9  J-1.il 

IBP  3  Cl, jf*TBP  1  II.J1  »T  RP3  II,  J]  »AK8KT  (I,  J1 
T.1P3  Jl.l)  -TBP3  |1,  I)  ♦COST  jl) 

IFJFT.LT.  5)  CALL  US  LF  S  (•  POD  T*  ,4  ,TBP3 , 7  ,  V ,  * ,  2) 

CALL  VC  FTPS  JTHF3,  N  ,  7,  Pol 

IPjKT.LT.5|  CALL  USdf  V  J*  FOOT  (St  »J  •  .  9,  PO,  N,  1 , 2) 

DO  11  1-1, PS 
L-L-1 

08V  JL|  -U0JI1 

DO  12  1*1.11 
DO  12  0-1,11 
L*L  ♦  1 

DRV  (L)-VD(I.J) 

DO  13  1-1. NS 
L-L*  1 

DPV (LI -POJI) 

I-JKT.IT.5)  CALL  OSKPV  (’DRV  •  ,  3, DRV,  HI  ,  1  .2) 

FET'JS  II 
END 
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CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
SE  NSITITITY  C  OV  A  RI  A  MCE  PROGRAH 


THIS  PROGEA  B  IS  USED  TO  S3LVE  THE  SRSOF.  SENSITIVITY 
EQUATIONS  JdZS  THERE  IS  AH  INCORRECT  I HRLIHERTAT IOH 
OF  DTNABICS  I*  THE  DESIGN  OF  THE  KALNAK  FILTER  .THE 
EQUATIONS  5  EC  OH  E 

pm:«  <f»-k*h)  p*P(p»-r^h)  t»df»+»tdpt»gqgt*k»bk*t 

VDOT=FY*7(Fn-  K*H>  T  +  UDFT-GQGT 
UDOT*  FU«UFT»GQGT 

THE  PRINCIPAL  PROGRAB  INPUTS  ARE  THE  POLLC8ING  CO¬ 
LLECTION  OF  SISTEH  AND  FILTER  NATRICES 


PO 

F 

F* 

H 

GQGT 

B 

K* 


THE  INITIAL  COVARIANCE  NAI  RI  X  (NXNI 
THE  TROTH  NODEL  DTNABICS  HATF.IX(NIN) 

THE  FILTER  NODEL  DTNANICS  BATRIX(NXW) 

THE  TRUTH  BODEL  REASURENEST  BATH  IX  (LIN)  ,  HHBRE 
L  IS  THE  HEASUREBENT  SECTOR  3IB2NSI01 
THE  INPUT  NOISE  COVARIANCE  NATRIX(NX) 

THE  HEASUREBENT  NOISE  COVARIANCE  BATRIX(LXL) 
FILTER  GAIN  (NIL) 


ccceccccrcccccccc._cccccccccccccc: 
c 
c 
c 
c 
c 
c 
c 


CCCECCCCCCCCCCCCCCCCCCCCCC 


THIS  PROGBAB  HAS  BEEN  DEVELOPED  USING  THE  IBSL  LIBRARY 
AVAILABLE  IN  THE  COBPUTEB  CENTER  OP  THE  NAVAL 
POSTGRADUATE  SCHOOL 

IBPLICIT  REALMS  (A-H.O-Z1 

n-.« . 

"FSSKH  (3 , 8)  ,H(  J,  3) 

CONNOi/KTR/N,  NS.NPD 
UIHENSION  FPU  LL  (8,8),  PSQR  (8  ) 

DIBEHSIOH  OPT  (8) 

DIB  F.N  SION  'J|J6),V(8,8),P  (3S)  .art  (361  .VDf  8,3)  , 

PVAP  (138)  ,3RV(136)  ,C  (CMf.  J*  (N36,  $f.?D(  J6) 

DIB  EN  Siofi  IBP)  (8,81  ,THP2  (8  8)  .t  NP3  (8,  81 
EQUIVALENCE  (U()f  .VAR  (33)  ,  I  V  (),  II  ,VAR  ( 3  7}  )  ,  IP  (1)  , 

AR  pOIJ  j.  (UC<f)  ,5rV(1))  .(  '0(1,  3  .DRV  (3  7i  J  ,  (rC(l(. 


®V 

ODRV 


[301 


S-ORDER  OP  THE  SYSTER  BODEL 
NP»  NUBBEP  OF  POINTS 

HPD-CONTBOl  or  INITIAL  diagnostic  output 

DT*TIHE  INTERVAL 

EXTERNAL  FUN 
CALL  UG ETI 0  (3,6,6) 

THE  rOLLOVING  SECTION  HEADS  THE  CPECIFIED  INTUT 
BATHICES.r,F*,GQGT,R!,8  AND  B 


S3 

97 


99 


READf’5.981  N.Nr.NPD,  Dt 
PORHATjJIS,  :30,5| 

'“TE  (6, 97)11,  NF  ,NPD  ,  OT 
fl  AT  ( •  3 • ,316, 3X,Gl6. 30) 


m 

POP. 

NS*N*  (S»l) /2 
N1»  NS»NS,'2*  3 
NV'i'-NStN'aZ 
FOR  B  AT<  8  P3  0 , 5) 


99 


nonnnnnnnnnr.nnononnrtnono 


nnnn 


DO  1  I-1.S 


'  A.  ■  m  •' 

SS*0(S.  )4)  <p  II.J1  ,j*i,») 

CALL 

do  2  i* 1.1 

SEaD(S,94)  (PS  (I.Jl.J-I.NI 
CALL  OSiiPI  (•PS*  ,  J.FS.B.B.N,  1) 
BEAD(S,99)  (GivVt(I)  ,1-1,5) 

CALL  0SJF7  (,GjGT*.4,GQGr,N,  1,1) 

DO  3  I-1.S 

BrAD{5,9$)  (AMX.J)  ,J-1,  3) 

CAL L  USWPSi^K'.I.Ak.a.S.J.I) 

DO  4  1-1.3 

DO  5  1-1.3 

BEAD(5.99|  (R  (I.J)  , J- 1,3 
CALL  OSUF!l  (*9*  ,  ><K,  3,3,3,  11 


20 


DO  4  1-1,136 
PAAIII -0. 

DO  7  T^>  1  ,N 
DO  7  J- 1  ,  » 

0PU,JI-fS 
CALL  OSfcfS 
CALL  VNULP 
CALL  PH’JLFP 
CALL  US-P*  ( 

CALCULATE  THE  DIPPESENC2  8ETHE2N  THE  DTNAHICS 
IF.PLEHEKTZO  IK  THE  PILTE8  AND  THE  PLANT,  DF-F"*- 

00  20  1-1,  « 

DO  20  J«1,N 
OFT  (I.J)  -OP  (I.J) 


KPh/'SeI  P^S.Isr.S.N.N.I) 

ULP?  (AIC.H,N,3,  J,d,3,TN?1  .9,  TEN) 

■JLFP  jT.-.P  1 .  AK  ,H  ,5, 5,5  ,3,*KPAT,3  ,  IER) 
HPE  ('KSKT'.a^iBKT^  ,N*N,1) 


OFT  (I.J)  -OP  (I.J) 

FT(f,J) -P(I,jf 

CALL  *TS ANX  (DfT  , N,  X  ,  3 ) 

CALL  US3F-  ( 'DLL  Ft'  ,6  ,  OFT,  3  ,N,N,  1) 
CALL  »TRANi(PT,ll.  B.  8) 

‘  OSW PN J • PT  , 3, FT, 9,N,N ,  1) 

VNULP  (AK,H,N,J,  N,9,3,  TNP1  ,8, 


CALL 

CALL 


IE9) 


21 


31 


32 


33 
1  1 


DO  21  1-1,8 
DO  21  J=1,8 

FSNKH  (I.J)  -PS  (I.J)  -TP  PI  (I,  J) 

FSiN  KHT  (I  ,  J)  -FSHKH(I.J) 

CALL  OPaFN  I'FS-BH*  ,  5,  PSHKH.  8,N,»,  1) 

CALL  VTBANX  (FS3RHT,  N,  N.81 

CALL  05-Fn  ('  (FS-AH)  T<  ,3,  PSP  KHT,  8  ,  N,  N,  1) 

t=o  . 

TOL - 1 .D-S 

INO-1 

L<=5 

IF(N.  EQ.8)  GO  TO  11 

DO  31  X-  1,  N  S 
L=L  ♦  1 

TAB  (L)  -0  (I) 

DO  32  1-1,* 

DO  32  J- 1 , N 
L-L  +  l 

TAB  (L)-V  (I.J) 

DO  3  3 
L-L  *  1 


1-1,  » 
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nnnn  n  n  nnnnn  non  noon  no 


TEND*FINAL  TI  BE 
TEN  0»  K*DT 

DVEPK  SUBROUTINE  FINDS  THE  SOICTION  OF  THE  SYSTEM  OF 
DIT  FERENTI  AL  EQUATIONS 

CALL  DVERK  (NV. FUN. T, VAR, TEND. TOL.IKD.C,  l36,W-'  ISB) 
IFfIND.  LE-O.OR.  IER.NE.OlSrOP 
CALL  VC  ITS  F  (VAB  <H  1)  ,N,PF0LL,8) 

CALCULATE  AND  PRINT  THE  HAS  ESTIMATE  ERPORS 

DO  30  1  =  1,  N 


REA  P*PrOlL  <1,11 
FFU  LL  (I  ,  I)  =  DABSJREAP) 

30  ?S0h<Il  =  CSQRT  (PFULL  |I,I|I 
WRITE  (6,93)  T,  jPSQMI)  .1*1. N) 


90 


ll'U*,1.U.H.31 
I  l,VAP(flS*1)  ,  8,11,11,31 

I  'P* .  l.WAB  11)  ,N.  3f 


IF  DESIRED  FRINT  THE 
CALL  05WSM 
CALL  USWFM 
CALL  USWSM 
10  CONTINUE 
STOP 
END 

SUBROUTINE  VTnANX  (A.U.NC.IA) 
IMPLICIT  BEAIP8  (A-H,0-Z) 


CIA. 7) 

COVARIANCE  MATRICES, P,U  AND 


DIB  ENSXON  A  <1  A, 


DO  1  1*1, N 
DO  1  J*1,N 
B  (I  ,  J 1 "  A  U.Il 

DO  2  1*1, N 

no  2  j*  i , n 

End 

SUS  ROUTINE  FUN(NV,T,VAR,DRT) 

FCN  SUBROUTINE  IS  USED  FOR  EVALUATING  FUNCTION S(INPUT) 


IMPLICIT  REALM  (A-H.O-Z) 

common  f  <8.81  .rsia.ef  ,C20T{81  .aaib.ji  ,r<3.3), 
•AMR  AT  <8.01  .  Dr  (8,3)  ,  FT  <8, 8)  ,  FSMRHT  <5 , 3  )  ,  OFT  (3,8)  , 
‘•FSHRH  8,8)  ,H(3,8[ 

COHMOH/RTR/N,  KS.NPD 

DIEEN510*  U  (Hi  .1  <8,0  I  .P  {J51 .00  (36)  ,V0  <8,B)  , 

PVAR  (1  3bl  .DRV  I  136)  ,C  (2UI  ,  WK  (  136.4)  ,P&(  36) 
DIMENSION  7M.P1  <8,6)  ,TM.f2  (8  ,  8)  ,  TMt>3  (8  ,  8) 

1-0 

DO  1  1*1, NS 
L*L»1 

1  U  (I)  *  VAB  (L) 

DO  2  I*1,S 
DO  l  J*1  ,H 

L*L  ♦  1 

2  V  <1  ,J)  *VAB  <l) 

DO  3  1*1, AS 
L»L  •  1 

3  1»  Cl )  -  TAP  j(L) 
ir<T.  EC.C)  AT»NrD 
AT*  RT»1 

AT. OF.  5)  GO  TO  15 


IF  <A  T.  GE.  5)  GO  TO  1 
Whl  TZ  <6 , 99 )  T 
99  FOB  HAT('0T*'»D25.  15) 
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IS 


CALL  OSHSMt'O'.I.O,  S,  3) 
call  os*fs  {•» ‘  • 
call  °sy,f5rlr«iiTi,,i3t 

<JALL  »?ULf5  (P.6, 


IHStksV  ,si£tF5*afrS-cWiin.TS:M.s .a. 

DO  5  1-1.  J 

22pi  a.J)  ♦!««  (i. 

cKL‘«i4?‘S«rMls2i®0PDT. 


3) 

3) 


'tall  5  s  h  r  n  t  •  P(PS-A«»  I  t’i“!*P2.fl.ii 
IrtKTlLT^i  tCA£L'u5MJ‘«('6?iv".l‘,.rS!>3.i,,,Hr3, 
DO  8  1-1. S 

e  ?“p“  «^Ij(s2"5U£-3»  ;T5P|  ‘I’ftpTOh?’ 

IFL(KT!LT.S?(cllL,SHlt»'(,»r»Df,.S.fnP»t«.ll.!l, 


9 

10 


DO  10  1-1.8 
2°„1  ,t*  ji  =T8P1  (I.  *T8P3  (I,  J)  ♦AKRKT  (1  ,  j  , 

. . . 

ipjkt.lt3s)  call'  ai ('•  pddt  (STH)  • , 9 .  rp . i  .  3) 

L-0 


DO  11  1-1.  *S 
L-L*  1 

11  PRT(L)«UD(I) 

DO  12  I-J.  J 
DO  12  J-1.P 
L-L*  1 

1  2  D81  (L)  -»D(t  ,J) 

DO  13  1*1.  "  S 
L*L*  1 
1  3  i*T  “ 


L*L*  1 

IPT  Sit^LT.’sl  CALL  US8P»fOSV,.3  .HR*."*.  1,3) 

pjruap 

iHD 
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